Estoy intentando implementar un simple bucle de cálculo de odometría. Que se está ejecutando en un bucle while. El siguiente es el código:
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);
}
He depurado el código con GDB. He comprobado las excepciones que se están planteando, pero todo parece estar bien. Y si cambio el código a esto:
dx = (double)(dt * v * arm_cos_f32(dw + dt*(w/2.0)));
dy = (double)(dt * v * arm_sin_f32(dw + dt*(w/2.0)));
La NaN desaparece. He intentado usar variables locales temporales. He comprobado las funciones cos
y sin
ejecutándolas en variables locales y funcionan según lo previsto. No hay problemas allí. Solo cuando quiero incrementar las variables globales, surge el problema. dx
, dy
y dw
son variables globales persistentes que hacen un seguimiento de la posición.
Esta es la versión de arm-gcc que estoy usando.
arm-none-eabi-gcc 5.4.1 20160609 (release) [ARM / embedded-5-branch revisión 237715]
Estoy utilizando la placa de descubrimiento STM32F429 con la cadena de herramientas Arm-gcc y se ha habilitado la FPU. Deshabilitar la FPU no tiene ningún efecto.
¿Algún puntero o sugerencia?