Hi! I have got some trouble with the compiler. My program compiles fine, but it hangs. Here is a link: http://mbed.org/users/BlazeX/code/CrashTest_AHRS/

It is using a 3D-Math library I have written years ago and used for PC-Apps. This lib I have ported to mbed and it es tested and running fine. So the lib is not the problem.

I want to implement this AHRS: https://github.com/TobiasSimon/MadgwickTests/blob/master/MadgwickAHRS.c Later I will use real sensors. But now it gets fixed values.

I have located the problem in the AHRS::MadgwickAHRSupdate method. If I access any variable the program hangs?!

If I comment out the MadgwickAHRSupdate in the AHRS::Update method it runs. If I comment out the hole code in MadgwickAHRSupdate it runs too. But if there is any variable in this methodes accessed, the program hangs.

EDIT: The problem is in the piece of code in the 2nd post.

Can someone help me please?

I have further reduced the problem. I kicked out the AHRS, the problem is somewhere else.

Here is the source code:

#include"mbed.h"

BusOut LEDs(LED1, LED2, LED3, LED4);

class Vector3
{
public:
    float x, y, z;
    
    Vector3()    {}
    Vector3(float all)  :    x(all),    y(all),    z(all)   {}
    Vector3(float _x, float _y, float _z)   :    x(_x),    y(_y),    z(_z)  {}
    Vector3(const Vector3 & v) :    x(v.x),    y(v.y),    z(v.z)    {}
};

#pragma pack(push, 1)
struct Input
{
    char x66;
    Vector3 Gyro;
    Vector3 Acc;
    Vector3 Mag;
} In;
#pragma pack(pop)

void Test2(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    In.Gyro.x= gx*gy*gz + ax*ay*az + mx*my*mz + 0.011;
}

void Test1(float dT, const Vector3 & Gyro, const Vector3 & Acc, const Vector3 & Mag)
{
    Test2(Gyro.x, Gyro.z, Gyro.y,
          Acc.x, Acc.z, Acc.y,
          Mag.x, Mag.z, Mag.y);
}


int main()
{
    In.Gyro= In.Acc= In.Mag= Vector3(0.123f);
        
    while(1)
    {
        LEDs= 0;
        
        //fixed values
        In.Gyro= Vector3(0,0,0.01f);
        In.Acc= Vector3(0);
        In.Mag= Vector3(0);
        
        LEDs= 1;       
        Test1(0.01f, In.Gyro, In.Acc, In.Mag);
        LEDs= 2;
        
        wait_ms(10);
    }
}

Well I've isolated the issue to the #pragma pack, I don't like replying until I know why something happening, but I thought I better had as you're currently working on it.

Well, you are right.

But I need to pack this structure, because of I is even bigger and only reduced for testing purposes. This structure is used for communication, and the x66 is the start marking byte.

I have implemented the protocol this way on the PC (MS VC++) and it works fine with pragma. So I can "write" the structure directly via Serial interface. This reduces bandwidth.

Compare this:

#include "mbed.h"

Serial PC(USBTX, USBRX);

#pragma pack(push, 1)
struct A
{
    char c;
    float f;
    short s;
    unsigned long l;
};
#pragma pack(pop)

struct B
{
    char c;
    float f;
    short s;
    unsigned long l;
};

int main()
{
    PC.baud(9600);
    PC.printf("sizeof(A)= %i, sizeof(B)= %i\r\n", sizeof(A), sizeof(B));

    while(1);
}

Yea I do the same with my serial communication packets, might be todo with the Vector class? I'm not sure yet

I have replaced the Vectors in the Input structure by simple floats. It works now.

But I think it is a compiler bug. The Vector class is a simple 12 byte block. 3 * 4byte float.

Well I should have refreshed this before, also found out the pragma is the issue :P

But is it really worth it just to save a tad of bandwidth? If I understand it correctly (didntk now it before), it makes sure there is no free space between the variables. However Vector3 are already 3 * 4 bytes nicely in a row. Then you got 3 of them. So if I am correct the difference is either sending 37 bytes, or 40 bytes, so it isnt that shocking.

I compared it at all levels of your original code, always 12 bytes as expected. Its odd but I'm not willing to blame the compiler just yet.

Could it be related to Vector3 being a union? When I used your GTMath library a bit ago I thought it was a bit of a dirty method, then again maybe it is perfectly normal, not that into such stuff.

Edit: Well if in both cases they are 12 bytes I guess not.

You're right about how pragma pack works, its usefulness comes from knowing that 3 floats a short and a a byte are stored in 15 bytes on both sides of communication, and you can just point a struct pointer at that memory and read/ write it. I still have no idea why it crashes on calling that function, makes no sense to me.

You win BlazeX, I'm going with compiler problem, doesn't like the floats not being aligned on a 4 byte boundary

Well, thanks for support!

So here is my application, I am currently working on: I want to build an autopilot system for RC planes/(multi-)coptors using 9DOF-AHRS at the moment, later GPS navigation. Because I cannot try and fail, I have to use a simulation before real flight. For development I want to use a Multiplex Twinstar 2 as platform, because it is easy to handle and hard to destroy. As simulator I am using Aerofly Professional Deluxe, with the Twinstar 2 model.

Now comes a bit of magic. A trainer-like program is reading the time, position, velocity and the orientation (attitude, heading) out of AFPDs memory, calculating sensor values for gyro, accelerometer, magnetometer and is transmitting theese data via USB serial interface to mbed. Mbed thinks this are real sensor values. Then it is using the open source AHRS to calculate Roll, Pitch and Yaw.

Here comes at the moment a very simple "autopilot": a simple P-controller for pitch and roll. Throttle and rudder are fixed. Its output was sent back to PC, instead of servos and ESC. Then to PPJoy and PPJoy's virtual device was configured to control the plane in AFPD.

The plane in the simulator is now stabilizing itself!

The artifical horizon is stable. The richt window shows the PPJoy channels: throttle, aileron, x, x, x, elevator, rudder, x.

That's a nice simulator setup, just a warning though sensors never work that smoothly in the real world, I built a AHRS for my quadcopter with the same sensors, although my simulator was strapping it between 2 chairs and seeing if it stabilised on that axis, (was fun when it escaped), my main issue was vibration and the magnetometer was a pain too, oh and ground effect, probably wont be as much an issue in a fixed wing aircraft =), hope you have fun on your project

Chris

Hi!

After a programming pause I have tried my "Crashtest" again and it still does not work. Here the link again: https://mbed.org/users/BlazeX/code/CrashTest_AHRS/

Try it out! It will hang. It runs great, when removing the #pragma lines.

Please help!

I would expect this code to cause an alignment fault when you try to access a 4-byte float at an address which isn't an even multiple of 4. This is not a compiler bug but a programming bug. It works on the PC side because x86 always performs alignment fix ups on such requests (with a potential performance degradation). The Cortex-M3 can only do such fix ups for a few of the load and store instructions. The rest will generate such an alignment fault. In the case of your data, I recommend removing the x66 field from your structure, send that mark byte by itself over the serial port and then send your structure with properly aligned floats. Then you save your bandwidth and get proper alignment. The extra overhead of making two sends calls should be insignificant to the cost of sending data over a serial connection.

Hope that helps,

Adam