Stand alone FOC fork?

I intend to try to do exactly this. The current firmware is an amazing achievement, but from a maintenance point of view, it's a nightmare. Everything is intertwined, volatile is used indesciminately instead of thought-through use of synchronization primitives (this is particularly nasty), etc.

What's needed is a separation of functionality: a framework for different sensor types (with self-contained code for each specific sensor), a generic implementation of controllers (PID and/or others), separate access functions for the PWM and sample timers... I've done some preliminary attempts at this, mostly to familiarise myself with the code, and I'm leaning towards your suggested method of re-implementing a subset of the functionality to get going. It's incredibly hard work to figure out what some of the code does. Try to figure out correct_hall() in mcpwm_foc.c for example... The problem there is that it mixes specialized initialization behaviour and decisions on what sensor to use (hall or observer?) with hall interpolation. It's almost impossible to maintain in the long run - my hat's off to Benjamin for managing it so far!

At the moment I'm trying to put together a dynamometer so that I can script a test suite and do regression testing. I think that's a requisite for any serious work on the firmware.

It's slow going though since I have ME/CFS and thus very little energy for this.

BTW, when you say "lightweight", what does that mean to you, functionality-wise? Which features should be included, what doesn't need to be included?

Unless I'm missing something, the observer code (observer_update() in mcpwm_foc.c) is actually very well documented through the PDF linked in the code, and also has no MCU dependencies. It does use global variables and it passes internal state variables as parameters, which are both not good practise. It should be one of the easier parts to modularize though (which would also take care of the global vars and state variable problems).

SVM and PID control are both used by the FOC code (and also in practice by the trapezoidal/BLDC code by the way, at least if you want motor current limiting), so in case you want motor control at all, SVM and PID modules need to be implemented/extracted first. Along with a FOC module? The algorithms for sensorless startup, motor parameter detection, and hall sensor detection should be extracted to their own modules.

I'm thinking things that could be left out would be the different control modes: duty cycle, speed, pos, braking. These are all intertwined with the pwm code today, and really shouldn't be. (Well, duty cycle in BLDC mode has to be of course.)

I wrote before that indiscriminate use of volatile instead of proper synchronization primitives is nasty. I'm trying to figure out what "proper synchronization primitives" means in ChibiOS/STM32F4 (which is an ARM Cortex-M4 processor and uses the Armv7-M instruction set, BTW).

For my own sake I feel I need to sum up here why volatile is bad and what is needed instead. Maybe others will find it interesting, too. :)

In C, 'volatile' is a type qualifier which forces the compiler to output instructions to actually read or write memory every time the variable is accessed (instead of e.g. placing the value in a register). It also makes the compiler respect order between volatile accesses. But volatile accesses are not atomic (i.e. they can be interrupted/preempted), nor do they ensure that the hardware executes memory accesses in order. They also (obviously) doesn't make related assignments to different variables atomic (e.g. updates to i_alpha and i_beta in the VESC firmware).

Since most things happen in the ADC interrupt service routine in the VESC firmware anyway, most of the problems associated with using volatile for synchronization (but also most of the cause for using it) should be void. But I want to "untangle" the firmware so that, for example, only the things that really have to be done in the ISR is done there. I want to enable sensor input using interrupts, running position and speed loops at lower frequency than the PWM frequency, etc. This will require proper synchronization.

As I understand it, proper synchronization on an ARM v7 CPU requires three things:

• A compiler barrier that prevents the compiler from reordering instructions over the barrier, and makes it flush data held in registers.
• A memory barrier that prevents the hardware from rearranging memory accesses over the barrier. (Also instruction barrier?)
• Code to prevent preemption while the critical section is executed.

But ChibiOS doesn't seem to have this proper implementation. There are mutexes, which are not usable from ISRs. There are also semaphores, but they are primarily meant for signalling between producers and consumers. Strangely it seems they don't make use of barriers (i.e. they only implement the last point above). There is the chSysLock() function, but looking at the implementation it just disables interrupts on ARMCMx (while on AVR it actually inserts a compiler barrier, as it should). It seems ChibiOS is severely lacking here?

The Linux kernel implements a barrier for STM32. It looks like this (from arch/arm/include/asm/barrier.h):

asm volatile ("DSB" : : : "memory");

This is a combination of a compiler barrier and the DSB ("Data Synchronization Barrier") instruction.

Here's a little background about the DSB instruction on ARM v7: http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka14041.html

Any ideas? I'm leaning towards writing my own code for critical sections. But it can be tricky getting it right!

I personal would love to see the back of ChibiOS. I get the advantages that it brings, but I don't feel that they're really all that helpful for an ESC.

A simple FOC implementation can of course be made, and there is a lot of information out there on how to do that. I could probably make a motor spin using FOC on any STM32 in a day or two with 5% of the VESC code. The difficult part in order to make it work for the first time without experience is to get all the details right. A wrong sign, a bad scaling, a missing SQRT(3) or many other tiny mistake will just prevent it from working at all, which is a big threshold in the beginning. Then when the motor can spin and works well enough for a simple pump application, all other applications make things more and more difficult to be handled by the same implementation.

The difficulty in the VESC code is really to get all the details right under all operating conditions with all motors in all applications. For example, when the motor is already moving when starting but undriven, the observer has to use different sources of information to track the motor, and the PWM modulation has to be set up exactly right to make a smooth transition. The temperature backoffs and detection functions for motor parameters and encoders are also tricky. There is not any special algorithm in the VESC that makes it work well, it is hundreds of small details.

I agree that the code can be cleaned up, the main problem is that I don't have much time and too much work to do. A proper test bench where testing of all corner cases can be done would also help a lot, which is something that I currently don't have set up.

Regarding the use of volatile, it is not about being atomic, it is about not optimizing certain things away where threads and interrupts share variables and need to be synchronized, which is something that chibios does not support too well. For example, things like this

for (float i = 0.0;i < 2.0 * M_PI;i += (2.0 * M_PI) / 500.0) {
		m_phase_now_override = i;
		chThdSleepMilliseconds(1);
}

would sometimes not work properly as the optimization would omit writing i to m_phase_now_override every iteration and instead do it at the end. In general I have made state variables that are shared between interrupts and the rest of the file volatile to avoid this. By analyzing everything most of the voltatiles can probably be removed, or completely different ways to synchronize things can be used, but I haven't given it that much priority.

Some functions have functionality that can be moved outside, such as the interpolation in correct_hall, but that interpolation is so far only used there and I did not find it worth the effort to move it out and make it general before using it in a similar way in other places. Functions and macros that are used in several places are in the utils files. I also made an effort to put common motor control functionality that can be abstracted into mc_interface, such as the backoff functions, ah counters, rt data filtering etc.

The PID controllers are used in several places, but the implementation are quite specific with many corner cases. The most simple PID controller can be implemented in a few lines of code, such as this

previous_error = 0
integral = 0
loop:
  error = setpoint - measured_value
  integral = integral + error * dt
  derivative = (error - previous_error) / dt
  output = Kp * error + Ki * integral + Kd * derivative
  previous_error = error
  wait(dt)
  goto loop

This could be put in a function, where the inputs are the setpoint, the current value, the control parameters and the time since the last execution. Then the function would keep track of the state, or the state could be allocated beforehand and passed to the function on every execution which is cleaner because it avoids side effects. In some cases it is not enough to just pass the state, the state also has to be altered based on conditions in the system. For example, the duty-based speed controller in the BLDC implementation has to set the integrator to the duty cycle with some scaling when it is activated at speed to get a smooth start, and not a jump. The state of the PI controllers and the order of operations in the control_current function also is very sensitive to initial conditions when starting the controller while the motor already is at speed. Taking all of this into account, the generalized controller can become quite complicated to work with, with all special cases, and every time the most demanding case is updated (such as control_current which I have updated a lot of times to make things smooth) it will affect the usage everywhere else. Therefore my preference has been to make every PID controller in place, as the most basic implementation is simpler to understand and see than the generalization for all cases used in the code.

Going back to the correct_hall function, it does not only take care of interpolation, it also handles transitioning between sensored and sensorless operation, moving to sensorless when hall sensors give invalid values, hysteresis between transitions, behaving correctly when the motor is run in open loop and other things. Making all of this general would make that function explode into different files and me putting effort into making nice interfaces for many things that are only used once in a specific way.

The problem there is that it mixes specialized initialization behaviour and decisions on what sensor to use (hall or observer?) with hall interpolation. It's almost impossible to maintain in the long run - my hat's off to Benjamin for managing it so far!

Reading things like this is not very encouraging. I have put in a lot of effort into making the best implementation decisions given the time I have available, and tried to generalize everything that makes sense generalizing. Many things have very specific details, and making them general while making room for the details would make things explode. As far as I know there is nothing major in the code that would pay off generalizing any time soon in future work with even close to the amount of work that goes into making it general.

Hi Benjamin, and thanks for the answer! I wrote above that I think the VESC firmware is an amazing achievement, and I mean that! It's probably at the limit of what can reasonably be expected from one very dedicated person! So please don't feel discouraged. But since you open sourced it, we have the possibility to make it even better!

I realise its functionality is very complex - but that is a reason for data encapsulation and modularization, not against it! (Data encapsulation allows generalization, but encapsulation is useful even if generalization is not desirable, since it improves code clarity.)

Note that I'm also not saying that I think restructuring of the code should be your top priority. (Hey, that 18 FET, 75 V, 300 A VESC seems like a worthwhile project! ). I'm saying this is something I'd like to work at. I'm also not trying to compete with you - the best possible outcome would be if I (or someone else) did the work, you took a look at it and said "Wow, this will make further development so much easier!" and ended up releasing it as a new major version of the VESC firmware.

Let's face it: it's really difficult to make changes to the current code, and understand how that will affect the whole. Say I wanted to add support for a new sensor, or make a change to how sensor data is combined. Since everything is in the same file, and makes plenty use of global variables, it's hard to know where to even begin - and even harder to ensure the changes doesn't have unintended side effects.

And I  think that's where lizardmech's request for a "version of the FOC code to speed up development and aid in adapting it to different projects" comes in. In the end it doesn't even have to have less features than the current firmware. Heck, look at the Linux kernel. That is an incredibly complex behemoth, but since it's well structured, it's relatively easy to work on! Even by lots of people simultaneously!

A quick example of what kind of restructuring I'm talking about. Take mcpwm_foc_hall_detect(). It uses a set of global variables (m_phase_override, m_phase_now_override, m_id_set, m_iq_set, m_control_mode, and m_state) to tell other code to listen to it instead of listening to the "normal" setpoints. This other code has to handle the special case of the override. mcpwm_foc_hall_detect() writes the detected data to hall_table[] which is returned to commands_process_packet() in commands.c which in turn writes it to a configuration(?) and returns it to VESC Tool(?).

What if instead it worked like this: hall_detect(hall, controller) in hall.c was called. "hall" is of type "struct hall" and (as you say above) contains the internal state of the hall sensor (i.e. its hall_table). "controller" is a "struct controller" containing, among other things, a method set_phase(angle, current) that tells the controller (be it SVM or BLDC) to set the stator currents to that angle and current. So hall_detect() could iterate over controller->set_phase(angle, current), recording the hall values as it went along into its internal state struct. Two things are won here: hall internal state is kept in one place, and the controller code doesn't have to bother with the special case of sensor detection. (commands_process_packet() could of course call hall_get_raw_table(hall, &table) or similar to make it possible to see the results of the detection in VESC Tool, but the ownership of the data would be clear.)

In the same way, speed and position controllers could be "extracted" to use a general interface to set the motor current. The code would be a lot simpler to understand and maintain, and would work at least as well.

This is my vision at least; one that makes me excited about working on this.

That's a good point about keeping things such as hall and observer together. I think users here can probably manage to do it without taking up your time if pointed in the right direction the hard part is getting the initial plan and realistic goals.

No, that's actually a very good reason to encapsulate them as much as possible, so that the needed interactions becomes explicit and obvious! Just saying... ;)

Thanks for the feedback, it is actually nice that others than me are looking at the code and suggesting improvements.

- How critical is chibiOS when it comes to the FOC motor control portions? Is taking the FOC code and setting it up to run without a RTOS a realistic goal?

The code did actually not use chibios a long time ago, but there are many advantages to using it and chibios is actually really nice and convenient to work with. Writing custom applications becomes much easier, and handling all the applications, communication interfaces, hw drivers with DMA, interrupts etc. becomes easier. For example, this would not be possible in the same way without chibios:

http://vedder.se/2015/08/vesc-writing-custom-applications/

Say I wanted to add support for a new sensor, or make a change to how sensor data is combined. Since everything is in the same file, and makes plenty use of global variables, it's hard to know where to even begin - and even harder to ensure the changes doesn't have unintended side effects.

Adding a new type of encoder can be done without changing mcpwm_foc.c at all. encoder.c is responsible for giving an encoder reading in the range 0 - 360 degrees when asked for it, and it has some functionality to deal with index pulses and wether or not they are found. The tricky and specific parts with encoders is that one encoder revolution most of the time is not mapped to one electrical motor revolution, and the position controller works in terms of encoder revolutions to not throw away information. This requires some scaling and mapping that is quite specific to how the motor control works internally, which is why I put it there. This way new encoders can be implemented without dealing with those specifics, as long as their position can be read as a float in the range of 0 - 360 degrees. I don't think it is worth moving out the encoder detection of mcpwm.c now, as it is so deeply connected.

Hall sensors could maybe be seen as an encoder with the lowest possible resolution to work at all, but by handling them as a special case gives better performance, which I found important as they are quite common and an extreme case of an encoder. I don't think it would make sense to add some sensor with just slightly higher resolution than hall sensors, which is why all other sensors in the future most likely fit nicely into the encoder implementation.

A quick example of what kind of restructuring I'm talking about. Take mcpwm_foc_hall_detect(). It uses a set of global variables (m_phase_override, m_phase_now_override, m_id_set, m_iq_set, m_control_mode, and m_state) to tell other code to listen to it instead of listening to the "normal" setpoints. This other code has to handle the special case of the override. mcpwm_foc_hall_detect() writes the detected data to hall_table[] which is returned to commands_process_packet() in commands.c which in turn writes it to a configuration(?) and returns it to VESC Tool(?).

What if instead it worked like this: hall_detect(hall, controller) in hall.c was called. "hall" is of type "struct hall" and (as you say above) contains the internal state of the hall sensor (i.e. its hall_table). "controller" is a "struct controller" containing, among other things, a method set_phase(angle, current) that tells the controller (be it SVM or BLDC) to set the stator currents to that angle and current. So hall_detect() could iterate over controller->set_phase(angle, current), recording the hall values as it went along into its internal state struct. Two things are won here: hall internal state is kept in one place, and the controller code doesn't have to bother with the special case of sensor detection. (commands_process_packet() could of course call hall_get_raw_table(hall, &table) or similar to make it possible to see the results of the detection in VESC Tool, but the ownership of the data would be clear.)

The hall table is owned by mc_interface.c (as all other settings in the motor configuration) and conf_general.c handles storing and reading them from flash (to retain them through power cycles) as well as providing default values for everything, which also can be overridden as a specific file generated from VESC Tool if desired. mcpwm_foc and mcpwm are not allowed to change the configuration (although they do it temporally in a few places and change it back while locking the interface, could be done nicer). The hall_detect function returns a hall table that is passed to VESC Tool. Vesc Tool is then responsible for putting it into the configuration and writing it back, as with all the other detected settings.

The state variables (which are not global, they are only visible in that file) that are changed to do sensor detection are not part of any control mode, and quite specific to that task. Making them part of the interface allowing to make sensor detection from the outside would as it looks now only be useful for that specific task, which is why I haven't put effort into doing that yet.

The hall detect function is a lot simpler than the encoder detection, but I still think it makes sense to keep it in mcpwm.c. That is because the hall table is specific to the foc implementation, and bldc has a different type of hall table in the configuration. (and yes, the BLDC implementation is a mess that needs to be cleaned up, but my focus has been on FOC for a while).

lizardmech: I don't see how removing ChibiOS and writing the corresponding code manually would make porting to another MCU easier? You would have to rewrite that code anyway for the new MCU... Also note that most of the hardware setup seems to be done through the STM32F4 Standard Peripherals Library, which is not really part of ChibiOS. But again, even if you wrote all that code by hand to get rid of the dependency, you'd have to rewrite it all for a new MCU family anyway.

Also, I have the feeling that you (or rather your programmers) blame ChibiOS for things that aren't ChibiOS's fault. It's not that difficult to understand ChibiOS - all the code is there, and it's a pretty thin layer. It's probably that they are unfamiliar with the VESC firmware in general that's the problem. Which IMU unit are you using?

Benjamin:

I don't think it is worth moving out the encoder detection of mcpwm.c now, as it is so deeply connected.

It's exactly because it's so deeply connected that it would be worthwile extracting it. It would be a PITA to do, but it would make the code much clearer and easier to maintain.

But I digress; I can see you're not convinced. :)

The state variables that are changed to do sensor detection are not part of any control mode

I know it isn't implemented as an externally accessible control mode, but isn't this pretty much the "handbrake" mode? I.e. run the motor openloop with a specified current (and phase). Or put othwerwise: both handbrake and sensor detection could be run through the same set_phase() method.

The hall table is owned by mc_interface.c (as all other settings in the motor configuration) and conf_general.c handles storing and reading them from flash

Yes, I can see now the rationale behind keeping all the configuration data in one place. Thanks for making that clear to me. It is unfortunate however, since it lands us in the exact opposite corner of the data encapsulation paradigm (object orientation) which is considered good practice (and for good reasons!). It would probably have been better to encapsulate data and have each object supply its own defaults as well as serialise it for flash storage. But it's always easy to be wise after the event...

The state variables (which are not global, they are only visible in that file)

In my experience, C variables that are declared to be visible within an entire compilation unit (.c file) and have the same lifetime as the program, are considered global (i.e. they don't have to have external linkage). But regardless of terminology, IMO the fact that these variables are "globally accessible within that file" is an important part of what makes it so hard to see what's going on.

Initialisation of the STM32F4 pins that your IMU is connected to, as well as any initialisation required by the IMU itself, would be placed in app_<name>_start() in applications/app_<name>.c, where <name> is a name of your choosing. If the IMU was very complicated to setup/use you could place device-specific code in a separate file and call it from app_<name>_start().

In app_<name>_start() you would also create and start a thread that would run your control algorithm (read IMU data, run your PID controller, tell the VESC what current to use by calling mc_interface_set_current(), and then sleep for a little while). Look at e.g. applications/app_ppm.c for an example. This whole process it greatly simplified by ChibiOS.

You could either replace <name> with "custom" and set custom app in VESC Tool, or modify app.c to start an app with a name of your choosing.

Regarding openocd, that's the program used to upload the binary image to the chip (when you run 'make upload'). I don't think it's required to build/use ChibiOS (or the VESC firmware altogether)?

It's exactly because it's so deeply connected that it would be worthwile extracting it. It would be a PITA to do, but it would make the code much clearer and easier to maintain.

But I digress; I can see you're not convinced. :)

Yes, I'm not convinced :-)

The foc hall sensor table is only relevant to my particular foc implementation, and not very useful in general. It is not even useful for my particular bldc implementation. Moving my foc hall sensor implementation into a seperate file would only reduce mcpwm_foc with a few lines, and add more code in total to glue everything together.

I know it isn't implemented as an externally accessible control mode, but isn't this pretty much the "handbrake" mode? I.e. run the motor openloop with a specified current (and phase). Or put othwerwise: both handbrake and sensor detection could be run through the same set_phase() method.

Yes, the hall sensor detection would work with an extended version of the hand brake (not only the phase, it also sets the current in the D axis instead of the Q axis), but the encoder detection for example requires much more. If I find a reason to make a handbrake function with control over the phase and both the D and Q axis currents, I might make an interface for that and make the hall sensor detection use it. Though I think that adding the axis to the handbrake function would only make it unneccessarily confusing to be used from apps.

Yes, I can see now the rationale behind keeping all the configuration data in one place. Thanks for making that clear to me. It is unfortunate however, since it lands us in the exact opposite corner of the data encapsulation paradigm (object orientation) which is considered good practice (and for good reasons!). It would probably have been better to encapsulate data and have each object supply its own defaults as well as serialise it for flash storage. But it's always easy to be wise after the event...

I don't agree here at all...

Yes, mcpwm could handle its own settings, mcpwm_foc could handle its own settings, mcinterface, encoder etc. as well. There is also a lot of overlap, which would lead to duplicate settings between the modules. Then serialization and deserialization could be handled with them separately, and the communication protocol would handle them separately with a bunch of new commands, and VESC Tool would handle them seperately or semi-seperately. Implementing that is a huge amount of work, and I think it would make dealing with vesc tool, the communication protocol and storing/sharing of settings more complicated with no real benefits.

Right now the settings are connected to a particular motor, which might be a DC motor or a PMSM motor driven by BLDC or FOC with or without different types of sensors. The different modules responsible for driving the motor have to look at the relevant settings for them. Not all settings are relevant for all modules, but there is a lot of overlap (current limits, temperature limits, voltage limits etc.). The settings don't store any state from mcpwm or mcpwm_foc either, they only tell them how to update their own state, which is stored in them internally. For example, the observer uses the settings to determine how and with which gains to update its state in every iteration.

To me this is a perfectly reasonable and sensible implementation. All settings that are related to the motor are stored in one place, and can be shared with all hardware that can be used to drive that motor. When I export settings from VESC Tool, I don't want to export combinations of 10 or so different structures, all related to which motor and sensors I'm using. The current structure is by far not where I would start refactoring things.

In my experience, C variables that are declared to be visible within an entire compilation unit (.c file) and have the same lifetime as the program, are considered global (i.e. they don't have to have external linkage). But regardless of terminology, IMO the fact that these variables are "globally accessible within that file" is an important part of what makes it so hard to see what's going on.

The static variables in mcpwm_foc module represent the state the motor in the view of that module, and the state of operation of the module itself. Internally the module keeps track of and updates its own state, and from the outside it provides interfaces for affecting its operation and reading relevant parts of the state in a general way. I see mcpwm_foc as one single module. Changing how it behaves internally requires deep knowledge of how it works, but that is the nature of the problem itself. Some of the state can probably be put into structures to make it a bit more clear, but I don't see anything major that I would change right away. When I'm working with the file I feel comfortable with it, and few times things get broken when I make updates. Also keep in mind that mcpwm_foc has A LOT more details and corner cases than literature about FOC and code examples usually provide, which naturally makes it more complicated to follow. For example, changing how duty cycle is controlled affects the PID speed controller when the speed reference is 0 which relies on a certain behavior, regardless if it is in the same file or abstracted away somewhere else. This kind of knowledge about nature of the problem itself is required by anyone dealing with the deepest parts of the algorithms.

By the way, one thing that could be split out is access to the hardware, but that is quite a bit of work and some things, such as the very hardware-specific inductance measurement, is not clear where to put. This does not have high priority now, but I could probably do it in a day or two.

@lizardmech

Regarding the IMU, the firmware will get support for it soon with a software I2C implementation to support an MPU6050, MPU9150 or MPU9250 on any two pins. A quaternion filter will also be implemented in the firmware, so that all relevant angles can be read out easily from apps.

Benjamin:

Yes, splitting up the config data while preserving the current code layout would be quite senseless. :)

Also I'm not saying there should be a "generic" hall sensor implementation - I'm arguing that encapsulating its data and implementation would be useful for the sake of code clarity. (I do think encapsulation of other things, like PID controllers, might bring further benefits of generalisation, even if they are used a bit differently in different places.)

I think, what I'm really trying to say is, that mcpwm_foc could be split up into several separate modules ("pid_speed", "sensor_hall", "sensor_observer", "pwm", etc.). The interactions could be extracted and implemented in other modules ("sensor_combined", "foc" - this latter would get the phase from sensor_combined->get_phase() and call something like pwm->set_phase()). (And I realise my example is simplified.) This would make the interactions explicit, and separate them from the implementation of the previous modules. And "pwm" here would be "general" in the sense of being useful for all different control modes as well as when detecting sensors and motor resistance; an app writer could use it if necessary, or use the higher level interface foc->set_current(), or the even higher pid_speed->set_speed().*

That is, split up the "nature of the problem" into smaller parts that are easier to grasp. Like the Unix philosophy of composability instead of monolithic design.

I'm not saying this is easy to do, and if you're comfortable working with the code as it is today then I can very well understand that it's not a high priority. Still, I think it could be very worthwhile if you want the project to "take on a life of its own" also on the software side.

I think I will stop trying to argue my point for now as to not waste both our times. If I ever manage to find the time & energy to show in practice that this works and have benefits, I'll bring it up again. :)

*I'm using "module->method" here to mean "a method belonging to that module"; the actual function name would be something like module_method() of course, since it's C we're talking about. :)