Forward acceleration, ie speed change compensation

Here is the place for all your dsPic or autopilot questions.

Moderator: lukasz

Forward acceleration, ie speed change compensation

Postby Polux rsv » Wed Jul 25, 2012 3:11 pm

Hi Tom,
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 :mrgreen:

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
Polux rsv
 
Posts: 2
Joined: Mon Jul 23, 2012 9:17 am

Re: Forward acceleration, ie speed change compensation

Postby Tom » Wed Jul 25, 2012 4:37 pm

Hello Angelo,

Very cool to hear you were able to port to code to an ARM!

Simple solution: on your motorbike the pitch angle will always be zero, which means the acceleration along the x-axis should be zero when you don't accelerate. Forward accelerations won't affect the centripetal acceleration. Just replace the x-axis accelerometer value with zero :-)
Hard solution: you should keep track of the change in speed from the GPS. this should be done in your GPS loop. The unit is (m/s)/s, so the change in speed per second. you can then simple substract that from the accelerometer value (of course divided by 9.81 to make sure you get the result in G).
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Forward acceleration, ie speed change compensation

Postby Polux rsv » Wed Jul 25, 2012 5:59 pm

Porting your code to ARM was as simple as copy-paste-compile-run. I included some files whithout any changes as I use gnu gcc compiler. :mrgreen:

Assuming the road is horizontal, and the bike is running normally, ie no front wheel leaving the ground, the code would be
Code: Select all
tmp1[0] = sensor_data.acc_x - (sensor_data.q*w/G + sin_pitch)-  gps.acc_x/G ;

is it correct?

But Pitch angle=0 is true as long as the road/track is horizontal. But this is never real. And the Ledenon, or Spa if you know it better, are good examples. And as I said, it could be nice to detect when front wheel leaves the ground. So I suspect Y and Z acc would affected by this. But again how ?

Angelo
Polux rsv
 
Posts: 2
Joined: Mon Jul 23, 2012 9:17 am

Re: Forward acceleration, ie speed change compensation

Postby Tom » Thu Jul 26, 2012 6:17 pm

Ok, well I assume your motor will only accelerate along the X-axis.

in sensors.c (gps task): something similar to:
double acceleration = (gps.speed_ms - last_speed) / dt; // dt is probably 0.2s
last_speed = speed_ms;

than in the kalman filter:

tmp1[0] = (sensor_data.acc_x - (acceleration / 9.81)) - (sensor_data.q*w/G + sin_pitch);


All untested, but you get the idea :-)
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Forward acceleration, ie speed change compensation

Postby zeeshan002 » Thu Apr 16, 2015 9:48 am

Hard solution: you should keep track of the change in speed from the GPS. this should be done in your GPS loop. The unit is (m/s)/s, so the change in speed per second. you can then simple substract that from the accelerometer value (of course divided by 9.81 to make sure you get the result in G).
zeeshan002
 
Posts: 1
Joined: Thu Apr 16, 2015 9:41 am

Re: Forward acceleration, ie speed change compensation

Postby Tom » Thu Apr 16, 2015 10:21 am

Yes, tried that. The problem is that this information is about 1 second delayed (you need 2 GPS readings, and GPS is already delayed) and the sensors need immediate compensation.
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium


Return to Firmware

Who is online

Users browsing this forum: Bing [Bot] and 1 guest

cron