code questions

[Roby]

 Hello Bill,

I hope all Matrixpilot fans are safe.

I have two questions regarding the code in rmat.c. In this line of code you multiply KPYAW by 8 and then in the same equation you divide by the same number. Can you please explain?

Also in this line why do you shift omegagyro[0] by 13 to the left, and divide spin_rate by 2?

Best Wished,

Roby

[william premerlani]

Hi Roby,

The general answers to questions of this type are:

1. MatrixPilot uses integer arithmetic as much as possible to provide plenty of spare CPU cycles to support features.

2. Many of the integer variables use Q2.14 format. The format native to the Microchip compiler is Q1.15. This requires binary point adjustments after multiply and divide operations.

3. We use both signed and unsigned data types. We also use the hardware divide instruction which treats both arguments as signed. So care must be taken to handle the case where one of the arguments is an unsigned value with the most significant bit set.

4. In order to achieve a good compromise between range and resolution, we use other binary representations besides Q2.14.

5. In some places we pair a multiply and divide operation if the pairing naturally appears in the math. The multiply operation produces a 32 bit intermediate result, then the divide operation produces a final 16 bit result. This improves overall resolution of the computation. However, care must be taken not to generate a math trap error under certain conditions, so special care must be taken with the binary points of everything involved.

Now, on to your specific questions.

Regarding the first question about multiplying KPYAW by 8 and dividing spin_rate by 8, this was done to increase the resolution of the integer factor in the multiply that was proportional to KPYAW by using some of the spare resolution in spin_rate, without changing the math. It gave 3 more bits resolution to the KPYAW factor. The compiler assigns an integer value at compile time, and does the multiply at run time.

With regard to your second question, the reason had to do with items 2 and 3 above.

Best regards,

Bill

--
--
---
You received this message because you are subscribed to the Google Groups "uavdevboard" group.
To unsubscribe from this group and stop receiving emails from it, send an email to uavdevboard...@googlegroups.com.
To view this discussion on the web, visit https://groups.google.com/d/msgid/uavdevboard/6a8b37ce-50f1-45fc-8022-94a93e4760a4o%40googlegroups.com.

[Roby]

Hello Bill,

Can you please give a numerical example for your answer above regarding KPYAW.

Regards,

Roby

[william premerlani]

Hi Roby,

Here is a numerical example.

For the UDB5, KPYAW is 1024 and DEGPERSEC is approximately 32. If we compute the first factor in the equation for the line that you reference, without the factor of 8,

which is KPYAW/(50*DEGPERSEC), as a real number the factor comes out to be 0.64, which the compiler truncates to zero when converting it to an unsigned integer.

If we actually used that value in the computation, the result would be nonsense.

spin_rate, on the other hand, will have plenty of non-zero bits, so I made the decision to take 3 bits from it and give it to the constant.

Multiplying the constant factor by 8 before converting to an integer produces the value 5.12, which gets truncated to 5 at compile time, which has 4 significant binary bits. The final result of the product of the constant*8 times spin_rate/8 will then be lowered by 2.3% from the ideal value, which is quite acceptable for the role of that particular line in the computation.

Best regards,

Bill

To view this discussion on the web, visit https://groups.google.com/d/msgid/uavdevboard/2821e52b-7078-4a24-85d9-d4db01afdbe7n%40googlegroups.com.

[Roby]

Hello Bill,

Perfectly clear, but I think you meant 3 significant binary bits instead of 4 in your explanation for representing 5.12.

On the other hand, in some parts of the code like  ''calib_accum = __builtin_mulsu(omegacorrPweighted[0], (uint16_t)(0.025*GGAIN/GYRO_CALIB_TAU)); " 
you use 0.025. Why don't you write it in fixed point. Unless you didn't write this part?

And also please give me an example of your comment in bullet "5" above, from the matrix pilot code, and where a math trap error condition may happen.

Best Regards,

Roby

[william premerlani]

Hi Roby,

Yes, you are correct, 3 bits not 4.

Regarding your question:

"On the other hand, in some parts of the code like  ''calib_accum = __builtin_mulsu(omegacorrPweighted[0], (uint16_t)(0.025*GGAIN/GYRO_CALIB_TAU)); " 
you use 0.025. Why don't you write it in fixed point. Unless you didn't write this part?"

I wrote that part.

The 0.025 is just one factor in an expression that computes a constant unsigned integer parameter that depends on three parameters. For convenience I use the compiler to compute the final result in a way that closely follows the math I used to figure out what needed to be done. The compiler starts with floating point values and then it converts to an unsigned integer at the end. That work is done at compile time and then at run time there is a simple integer multiply, there are no floating point operations. It is instructive to look at the assembly listing for the statement in question, below. The opcode MUL.SU executes an integer multiply of two 16 bit values, the first operand, W0, is treated as signed and the second one, W1 is treated as unsigned. The 32 bit signed result is placed in the register pair W1 W0. For typical values of GGAIN and GYRO_CALIB_TAU, the unsigned integer constant comes out to be 0x12, or 18.

Best regards,

Bill

734:               calib_accum = __builtin_mulsu(omegacorrPweighted[0], (uint16_t)(0.025*GGAIN/GYRO_CALIB_TAU));
00474A  470068     ADD W14, #0x8, W0
00474C  780010     MOV [W0], W0
00474E  200121     MOV #0x12, W1
004750  B90001     MUL.SU W0, W1, W0
004752  980710     MOV W0, [W14+2]
004754  980721     MOV W1, [W14+4]

To view this discussion on the web, visit https://groups.google.com/d/msgid/uavdevboard/cbd8a7a7-8574-4f37-a9cb-a5730fa60298n%40googlegroups.com.

[Roby]

Hi Bill,

So you are always optimizing the calculations as much as possible to minimize the processing burden on the CPU.

For example, you pre-multiply omegagyro[0] with 2**13 in this equation " spin_axis[0] = __builtin_divsd(((int32_t)omegagyro[0]) << 13, spin_rate_over_2); "

to yield the result already in Q15 format. Is this true ? You could have multiplied by 2**16 instead to gain maximum accuracy ?

Best Regards,

Roby

[Roby]

Hi Bill,

I have a second question regarding math trap errors you mentioned above. Do you mean we should take care of overflow errors in case of pairing multiply and divide operations.

Best Regards,

Roby

[william premerlani]

Hi Roby,

The family of dsPIC33 processors can produce high priority trap interrupts for the following conditions:

clock issues

stack overflow

address errors

math errors

direct memory access errors

Ideally you do not want any of those errors to ever occur in flight, so you carefully debug your firmware to avoid them.

You can write trap handlers for each of them, but really, there is not much you can do about them at run time, except for clock issues, which can be handled.

What it means is that if you get any of the above errors at runtime, chances are your controls are not going to work.

Included in the math errors that will induce a trap is a condition that can arise in one of the hardware divide instructions that starts with a 32 bit signed integer divided by a 16 bit signed integer and is supposed to produce a 16 bit signed result. You should make sure ahead of time that the result actually will fit in 16 bits, otherwise the hardware will generate a math trap error. In principle you might try to write a trap handler to figure out what went wrong and somehow work around it, but I would not recommend it.

In general, you might see some trap errors during development, but you would want to quickly figure out why they are happening and fix the root cause.

Best regards,

Bill

To view this discussion on the web, visit https://groups.google.com/d/msgid/uavdevboard/3a04148d-6142-4983-853b-edeaa1216015n%40googlegroups.com.