Jason Dorie's FC software

michaellangford99

For benchmarking purposes, how fast does Jason Dorie's flight controller poll the sensors?

Thanks!

JasonDorie

Sensors are polled at 500hz, set here: https://github.com/parallaxinc/Flight-Controller/blob/master/Firmware-C/sensors_driver.spin#L1106
The altimeter/pressure data is an exception, as it may not be available that often, so the status byte is checked. If a new reading is available, it's read and the pressure is converted to an altitude estimate.

The flight loop and IMU update runs slower than this - 250hz, as set here: https://github.com/parallaxinc/Flight-Controller/blob/master/Firmware-C/constants.h#L32

Having the sensors read more often means that the gyro/accel values are always within 1/500th of a second when the IMU is updated, so their lag is lower than if I had read them at 250hz. It's worth mentioning that even 250hz is probably overkill for a larger quad - more mass and a larger radius means that the quad will move slower and be less affected by wind or turbulence, so it doesn't need to react as quickly. I've seen them run at 100hz, or even 50hz with good flight characteristics. For a smaller more nimble quad, the higher rate is desirable.

michaellangford99

I am still using the spin example I2C code for the L3G4200D gyro on my quadcopter, (a small micro quadcopter). Should I rewrite this code in PASM to get the needed refresh speed? It seems that my control loop is reacting sluggishly. Thanks so much for the input - It's great getting the information directly from you.

JasonDorie

Yeah, Spin is going to be way too slow. The control loop itself might be ok, but reading the devices will take forever. Spin is about 400x slower than PASM, give or take. I wrote the FC in Spin originally, but switched to CMM compiled C because it was quite a bit faster (among other things).

michaellangford99

Thanks! I have gotten my gyro all the way up to 2Khz refresh rate, and my accelerometer to 10Khz with PASM. I still have yet to incorporate these drivers into my quadcopter code, as I still have to fix a problem with the accelerometer driver. I took the basic demo code, and encapsulated the spin driver code in assembly, using it to call the already built-in assembly spi functions. However, I cannot seem to write to the accelerometer's offset value register. I can write to the mode control register (which is necessary for selecting the G mode). Here is my PASM code.

Jason, would you recommend switching to C/C++, or should Spin be able to manage the flight loop?

P.S. let me know if this should be reposted into a completely new thread.

JasonDorie

Spin should be able to manage the flight loop itself, depending on how you run the math and how fast you want to update. The default Spin compiler does no optimization so you may need to tweak the code for speed. The Elev8-FC runs the IMU on its own cog, but the flight loop and PIDs all run on the main thread.

JasonDorie

Also, I just did the offsetting myself, and wrote the values into high eeprom so I could recall them on startup after a calibrate routine.

michaellangford99

I incorporated the new sensor drivers successfully, however, I can only get the flight loop to 176 Hz (sensors are in independant cogs). I am running two kalman filters, which is taking up most of the time. I am using lonesock's F32 lib. Should I switch to a fixed point library or write my own? In the filter there are decimal numbers like 0.1 that if are multiplied against a number would decrease its value(0.1 * 10 = 1), yet if that is scaled up that would not be true(1*100 = 100)

JasonDorie

I'd look into a complimentary filter instead of Kalman - they're a little easier to work with. That said, every Kalman filter "implementation" I've seen has been more or less a literal copy of one that was published, and they all contain a bunch of extra multiplication terms of fixed constants that are zero and one. I'd look through what you have to see if there are any obvious things like that you can remove.

X = X + (Y * 0.0) is just X
and
X = X + (Y * 1.0) is just X + Y

That kind of thing.

Spin is going to be the limiting factor here at some point - The F32 library rips through the floating point math, but Spin is required to set it up and get the answers back. I used the same F32 library in C++ for the flight controller, but I added the ability to set up a stream of instructions for it to process, and wrote it out as a sequence of bytes in memory that the F32 driver consumes as it processes. It's much faster, but it's a pain to work that way. I ended up writing a compiler to convert normal C/C++ expressions to the bytecode format I use so I didn't have to change it by hand.

michaellangford99

Thanks! I will look into that. I am also embedding my rolling average code (which is using %40 of the time in the sensor loop) in assembler. Is it possible to use your stream driver with spin? I will have a look at that driver - it sounds fascinating.

JasonDorie

I don't have a Spin version of the code to set it up, but the PASM driver part would be compatible with Spin. It's better suited to working in C than in Spin because I use an enum as a lookup into the array of variables. It's not required, but it makes the code more readable.

This is what the input to the float stream processor looks like:
https://github.com/parallaxinc/Flight-Controller/blob/FloatStreamCompiler/Firmware-C/QuatIMU_QuatUpdate.inc

The form is:
(opcode), (input1), (input2), (output)

Anything that looks like a label turns into a single constant byte value, so each instruction to process is 4 bytes. The (opcodes) are an index into the list of instructions supported by the F32 driver, and the (input) / (output) values are indices into an array of 32 bit values. Usually they're floats, but some are integers.

This is a list of labels and their indices in the float array:
https://github.com/parallaxinc/Flight-Controller/blob/FloatStreamCompiler/Firmware-C/QuatIMU_vars.inc

Using it looks like this:
https://github.com/parallaxinc/Flight-Controller/blob/FloatStreamCompiler/Firmware-C/quatimu.cpp#L350

I customized the driver a bit to pull out instructions I wasn't using, make room for some new ones, and add the stream processor.

michaellangford99

Thanks so much! I might be able to use constants as labels for an index into an array. I will see if I can make this work! Also, is spin slow enough that one could assume that the floating point operation was complete without busy waiting for a response?

PUB FAdd(a, b)
  result  := cmdFAdd
  f32_Cmd := @result
  repeat
  while f32_Cmd
          

would then become

PUB FAdd(a, b)
  result  := cmdFAdd
  f32_Cmd := @result

JasonDorie

I think it would depend on the operation. It's reasonably likely, but you might need to do a redundant operation in there just to make sure you gave yourself enough time. Something like f32_Cmd = f32_Cmd + 1 right before you overwrite it with the value from the F32 result.

The problem with doing this is that you'd still be using Spin to set up the operations and move the results around, and that's likely going to be your limiting factor. That said, it might be ok depending on how complicated the rest of your code is. I initially had code that updated an orientation matrix using fixed-point math, entirely in Spin, and I could run that at 250Hz. I had to work to get it that fast, but it's possible.

michaellangford99

Where would I find a resource for using spin and C together? And C and assembly? I got both gyro and accelerometer into completely separate asm drivers, bringing my flight loop to 430 Hz. My largest time drain is now the floating point math, but I am hopeful it will fly as is. I will be running a flight test very soon - I will keep you posted.

JasonDorie

Spin and C aren't easily compatible. You can use something like SpinToCpp to convert it, but unless you're working from an existing object I'd suggest just going straight to C/C++.

For C and ASM it's possible to use both, and you can even use Spin-based PASM objects. I do this a lot in the Elev8-FC code because I started with Spin and then converted to C/C++ after I was well into the project, and already had a bunch of assembly drivers. You need to write the "interface" code in C, but that's usually pretty thin. You can also use the GAS (Gcc ASsembler) format, but I didn't go that route. There are posts in the forums and probably stuff in the Learn docs on Parallax's site, but it's a more advanced topic so you'll have to dig a bit.

You might find it helpful to look at one of the simpler Elev8-FC drivers, like RC.cpp / and RC_driver_ppm.spin.
https://github.com/parallaxinc/Flight-Controller/tree/master/Firmware-C

SimpleIDE automatically extracts the PASM block from a Spin file and assembles it, and then you use the use_cog_driver and load_cog_driver macros to get the address of and launch the pasm code.

Look here:
https://github.com/parallaxinc/Flight-Controller/blob/9316b160db272c567c985d68add06d26c3283aa1/Firmware-C/rc.cpp#L42

...and here:
https://github.com/parallaxinc/Flight-Controller/blob/master/Firmware-C/rc_driver_ppm.spin

Note that all the Spin code in that file is ignored - the code in the cpp provides the same functionality.