displacement calculation bug

[tnicht]

I think there is a parenthesis error in the calculation of the displacement in the mpucontroller.cpp firmware code (line 995).
FILE:
https://github.com/kauailabs/navxmxp/blob/master/stm32/MPU9250/mpucontroller.cpp

This is the line (995):
pdata->displacement[i] += (((pdata->velocity[i] + (curr_velocity_m_s - pdata->velocity[i]))/2.0) * curr_sample_period);
Over one small dt time period, one should calculate Xfinal = Xinitial + ((Vfinal+Vintial)/2)*dt for trapezoidal integration. In the code, this is expanded as Xfinal = Xinitial + (Vinitial + (Vfinal-Vinitial)/2)*dt. (Note, the comment in line 994 is missing the "Xinitial" portion of the equation also, but this is not relevant to the actual code.)
If you follow the parentheses in the line 995 expression, it appears that what is being calculated is:
Xfinal = Xinitial + (((Vinitial + (Vfinal-Vinitial))/2)*dt // NOTE, this is wrong
The "/2" is being applied to the first initial velocity term as well, so the Vinitial ends up being subtracted out and one gets just (Vfinal/2)*dt being added to Xinitial. I suggest the change needed is:
NEW PROPOSE LINE 995:
pdata->displacement[i] += (pdata->velocity[i] + (curr_velocity_m_s - pdata->velocity[i])/2.0) * curr_sample_period);

I know there has been discussion about the accuracy (or lack thereof) of the displacement from this sensor board. This bug may be contributing to this.