I used your source code to implement your AHRS on my existing hardware based on LPC1769(It makes also other functions like CAN, GPS gateway,...). As I also use Freertos, I used both the sensors task and communication tasks. I am able to check everything with GCS
I use it on my race motorbike to know the lean angle. It work very well when riding smoothly and close to constant speed, like an RC airplane. But looking at the centripetal compensation code, it missed the velocity acceleration change, which will certainly impact pitch (roll ?) during heavy acceleration on track.
- Code: Select all
tmp1[0] = sensor_data.acc_x - (sensor_data.q*w/G + sin_pitch);
tmp1[1] = sensor_data.acc_y - ((sensor_data.r*u - sensor_data.p*w)/G - cos_pitch*sin_roll);
tmp1[2] = sensor_data.acc_z - ((sensor_data.p*w - sensor_data.q*u)/G - cos_pitch*cos_roll);
Using GPS with the idea comming from APM2 code, I calculate the forward acceleration. But at this point, with my poor knoweledge of math and geometry, I don't know how and where I should add/substract this acceleration. Intuitively, it should be substracted to acc_x, but what about y and z ? And what about sin/cos of pitch/roll ?
Could you help me with this ?
Many thanks.
Angelo