Improvement to Sonar pitch/roll compensation algorithm

[GoogleCodeExporter]

At present the sonar ranging calc in ArducopterMega.pde assumes a very narrow 
beam width, and multiplies by the cos of both the pitch and roll angle to 
estimate the altitude. However, the recommended Sonar (EZ0) model does not have 
a narrow beam width, and will see the ground at 20 to 30 degrees off normal at 
about the same distance as on the centre line. (See the linked datasheet on the 
Sonar wiki page)Therefore for this sonar, a 30 pitch or roll will not make much 
difference to the sonar reading, but the code will apply a 30 degree cos factor 
and hence underestimate the altitude by this amount. 

I'm using an EZ2, which does have a fairly narrow beam width, so the calcs are 
probably OK for that.

I think the cos * cos calc in the code needs to be augmented by some 
alternative code, (run according to a #define of Sonar model) so that it 
doesn't have this problem.

Looking at the datasheet, it may be as simple as subtracting say 20 degrees 
from the roll/pitch angle and doing nothing until this exceeds zero. Then do a 
cos on the excess and apply this factor. IE implement a deadband in the 
algorithm to allow for the 'half beam width'

In pseudocode:

temp_altitude = read.sonar

mod_roll = max(actual_roll, -actual_roll)   //find modulus of roll
mod_pitch= max(actual_pitch, -actual_pitch)

effective_roll = max(0, mod_roll-20 degrees)   // 20 degree deadband
effective_pitch = max(0, mod_pitch-20 degrees)
alt_compensation = max (0.707, cos(effective roll) * cos(effective pitch)       
                          //compensate outside deadband
altitude = temp-altitude * alt_compensation    //compensated altitude

I don't have an EZ0, but it would be easy for someone to do a test to see 
exactly how measured range falls off with angle, say as you point the sonar at 
the ground from a reasonable height and roll the craft.
Would someone like to do a graph please?
I shall do this for the EZ2 once I have installed it.

Original issue reported on code.google.com by goo...@harding99.freeserve.co.uk on 1 Jun 2011 at 9:28

[GoogleCodeExporter]

Correcion, for 'falls off' read 'increases' in last paragraph.

Original comment by goo...@harding99.freeserve.co.uk on 2 Jun 2011 at 11:58

[GoogleCodeExporter]

Looks good. Would you be able to provide a patch?

Original comment by jasonshort on 3 Jun 2011 at 3:35

[GoogleCodeExporter]

OK, I'll try to write some code for you.
I have measured the LV EZ2 sonar, and its deadband is about 4 degrees, before 
it starts increasing as the cosine. (ie there's about 4 degrees of gap between 
the ideal cosine curve and the measured curve.
Need some other folks to measure the EZ0 now please.

Original comment by goo...@harding99.freeserve.co.uk on 3 Jun 2011 at 6:28

[GoogleCodeExporter]

Jason, what's the best way to do this: Email you the suggested changes (I don't 
have your email), or Browse the source file in the wiki, click 'edit file', 
make the changes and then click 'upload patch' .
Will the change get moderated and checked then before being included? I'm not 
an authorised coder on this project.
regards
Rob

Original comment by goo...@harding99.freeserve.co.uk on 4 Jun 2011 at 5:01

[GoogleCodeExporter]

I've had a look in more detail at the code and it's not as easy as I expected. 
Sin and cos of the roll and pitch are available, but not the roll and pitch 
angles themselves. I assume we can't do sin and cos functions because they are 
too expensive? This makes manipulating the roll and pitch angle values in the 
way I proposed before doing the sin and cos tricky.
If you can see an easy way to implement what I was trying to achieve, please 
let me know, and I'll have another go.
Thanks Rob

Original comment by goo...@harding99.freeserve.co.uk on 5 Jun 2011 at 11:07

[GoogleCodeExporter]

dcm.roll_sensor = roll in degrees * 100
Would that help?

Original comment by jasonshort on 5 Jun 2011 at 11:10

[GoogleCodeExporter]

[deleted comment]

[GoogleCodeExporter]

Jason, please see attached file for proposed code changes,
regards
Rob

Original comment by goo...@harding99.freeserve.co.uk on 8 Jun 2011 at 10:26

Attachments:

• [Sonar attitude measurement changes.docx](https://storage.googleapis.com/google-code-attachments/arducopter/issue-181/comment-8/Sonar attitude measurement changes.docx)

[GoogleCodeExporter]

Sorry Jason, I know you are snowed under with support for drift and Yaw issues 
at the moment. This sonar issue is really not important in comparison, but it's 
here for when you get to it.
regards
Rob

Original comment by goo...@harding99.freeserve.co.uk on 8 Jun 2011 at 9:16

[GoogleCodeExporter]

Hi Jason, have you had a chance to look at the above code change proposal for 
inclusion in the imminent release? Thanks and regards
Rob

Original comment by goo...@harding99.freeserve.co.uk on 18 Jun 2011 at 8:19

[GoogleCodeExporter]

Hi Jason, I have just tested the recommended XL-Maxsonar-EZ0 for its behaviour 
when tilted to the ground, and it's even more flat that I expected. For example 
at a sonar distance of 75cm from the ground (with the quad held horizontal,) 
when tilted to 45 degrees the sonar reads 80 cm. ie it hardly changes at all. 
Another example 200 cm horizontal versus 210 cm at 45 degreees. Both tests done 
over short grass. This is way off a cosine curve, which would have predicted 
that the sonar should read 106 cm rather than 80, and 282 cm rather than 210.

Therefore we would be better off merely commenting out the line that scales the 
altitude by cos(roll) * cos(pitch), rather than implementing my code change 
above.

Could you comment out this line for the next release please?

(If you're not happy with this, it's actually pretty linear between 0 and 45 
degrees, increasing by around 6%, so could code that instead to be more 
accurate.)

Thanks Rob

Original comment by goo...@harding99.freeserve.co.uk on 18 Jun 2011 at 5:04

[GoogleCodeExporter]

sounds good!

Original comment by jasonshort on 18 Jun 2011 at 5:20

[GoogleCodeExporter]

Original comment by jasonshort on 1 Aug 2011 at 3:58

• Changed state: WontFix