2
\$\begingroup\$

I am trying to implement a simple Odometry computation loop. Which is being run in a while loop. Following is the code:

double dx=0,dy=0,dw=0;
double v,w;

void computeOdom(){

    uint32_t ticknow=ticksms;  //global timer variable (gives current ticks in milliseconds)

    v=0.0;
    w=0.0;

    double dt = (ticknow - tickprev)/1000.0; //time elapsed since last odom update
    double res = (double)ENCODER_RESOLUTION; //Encoder Resolution ticks per revolution
    double leftvel = (double) ((((Left.position)/res) * 2 * M_PI))/dt; // Angular velocity of left wheel (rad/s)

    Left.position=0;
    double rightvel=(((double)Right.position)/ENCODER_RESOLUTION)* 2 * M_PI/dt; //Angular velocity of right wheel (rad/s)

    Right.position=0;

    double r_L= 0.260/2;
    double r_R= 0.260/2;

    v += (r_L/2.0) * leftvel; //Linear velocity of the robot
    v += (r_R/2.0) * rightvel;
    w += (r_R/0.610) * rightvel; //Angular velocity of the robot
    w -= (r_L/0.610) * leftvel;
    double tempx=0.0,tempy=0.0,tempz=0.0;

    if(!(w==0.0f)){
        dx += (double)(dt * v * arm_cos_f32(dw + dt*(w/2.0))); //The line where dx becomes NaN
        dy += (double)(dt * v * arm_sin_f32(dw + dt*(w/2.0))); //The line where dy becomes NaN
        dw += (dt * w);
    }
    else{
        dx += (double)(dt * v * arm_cos_f32(dw));
        dy += (double)(dt * v * arm_sin_f32(dw));
    }

    tickprev = ticknow;

    printf("prev_x: %f\n\r",dx);
}

I have debugged the code with GDB. I have checked for any exceptions that are being raised but everything looks fine. And if I change the code to this:

dx = (double)(dt * v * arm_cos_f32(dw + dt*(w/2.0))); 
dy = (double)(dt * v * arm_sin_f32(dw + dt*(w/2.0)));

The NaN goes away. I have tried using temporary local variables. I have checked the cos and sin functions by executing them on local variables and they work as intended. No issues there. Only when I want to increment the global variables, the issue arises. dx, dy and dw are persistent global variables which keep track of the position.

This is the version of arm-gcc I am using.

arm-none-eabi-gcc 5.4.1 20160609 (release) [ARM/embedded-5-branch revision 237715]

I am using STM32F429 discovery board with Arm-gcc toolchain and FPU has been enabled. Disabling the FPU has no effect whatsoever.

Any pointers or suggestions?

\$\endgroup\$
  • \$\begingroup\$ Sorry forgot to mention. I am using STM32F429 discovery board with Arm-gcc toolchain and FPU has been enabled. Disabling the FPU has no effect whatsoever. \$\endgroup\$ – Asusrog Nov 25 '16 at 16:29
  • \$\begingroup\$ Use float not double if you have FPU. double is not floating point, that's why FPU does not have any effect. \$\endgroup\$ – Bence Kaulics Nov 25 '16 at 16:38
  • 1
    \$\begingroup\$ I've seen compilers do that.. But I just checked and arm-gcc treats doubles as 64-bit data.. \$\endgroup\$ – m.Alin Nov 25 '16 at 17:05
  • 3
    \$\begingroup\$ This is the reason of my advice. "Floating point numbers can be single precision (“float” in C) or double precision (“double” in C). The FPU in the Cortex-M4 processor supports single precision operations, but not double. If double precision calculation is present, the C compiler will use runtime library functions to handle the calculation in software. For best performance, it is best to handle the calculation in single precision if possible." from \$\endgroup\$ – Bence Kaulics Nov 25 '16 at 17:10
  • 1
    \$\begingroup\$ @Asusrog Sorry I was not suggesting it as a solution that's why I just posted it as a comment. I just wanted to call your attention to that you should use float types to get the adventage of the FPU. \$\endgroup\$ – Bence Kaulics Nov 26 '16 at 10:14
1
\$\begingroup\$

@MarkoBursic Thank you for pointing those out man. You know, actually i made so many changes in that tiny piece of code since yesterday that i started losing track of the idioticity :). Anyhow i have somehow managed to solve the problem. Bear with me for a while. using gdb i stepped through the code once again, checking each variable before and after every line of execution. leftvel was initially a nan(0x400000) when dt was zero since during the first iteration tickprev and ticknow were the same (0). Since leftvel and rightvel are local variables the nan values didnt stick through. And this nan value was getting multiplied to my global variables which are persistent. So i added this line :

if(isnan(leftvel) || isnan(rightvel))
    return;

Just before multiplying it with dx,dy and dw. Thank you for your suggestions @MarcoBursic @BenceKaulics

\$\endgroup\$
0
\$\begingroup\$

Read it as longer comment:

Why do you constantly parse the results to double? Many intermediate results could be set to const.
double res = (double)ENCODER_RESOLUTION; ?? 1st waste of time,
double rightvel=(((double)Right.position)/ENCODER_RESOLUTION)* 2 * M_PI/dt; 2nd waste of time,

This should be the same, but it is:

double leftvel = (double) ((((Left.position)/res) * 2 * M_PI))/dt;   

2 * M_PI/ENCODER_RESOLUTION could be a const.

uint32_t ticknow=ticksms;   
double dt = (ticknow - tickprev)/1000.0; //time elapsed since last odom update  

Look this, you have uint32 , then you divide by float number 1000.0 and you expect double. double((ticknow - tickprev))/1000.0 . You are parsing where is not necessarily, but you forget to parse where you should.

\$\endgroup\$
  • \$\begingroup\$ There is nothing wrong with (ticknow - tickprev)/1000.0. The subtraction is done on uint32_t which seems fine. The result of the subtraction is type uint32_t. This then gets balanced to double, because 1000.0 is a double literal (not float). No need to change anything. And I have no idea what double((ticknow - tickprev))/1000.0 is supposed to mean but it isn't valid C. \$\endgroup\$ – Lundin Nov 30 '16 at 12:02

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.