On a separate line of code, there is a working baro/sonar alt hold. Using my MaxBotix MB1200 sonar this works really well when the alt is in sonar range. Anywhere out of that range and the baro code is really erratic. Probably does not help that there are trees around me and the sonar is probably bouncing lower than actual cm to the code as the hexacopter floats above the trees.
So two separate code bases which do great jobs for either sonar or baro alt hold, just not both.
I did try Multiwii_r1311_NHA_r13_COMPLETE with just baro and that is great, but then I thought I would try to replace the BaroAlt value with the sonarAlt value when BaroAlt < 300 and sonarAlt < 300 -
Code: Select all
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = log( baroGroundPressure / (baroPressureSum/(float)(BARO_TAB_SIZE - 1)) ) * (baroTemperature+27315) * 29.271267f; // in cemtimeter
if ((BaroAlt < 300) && (sonarAlt < 300))
BaroAlt = sonarAlt;
debug = (int16_t)BaroAlt;
debug = sonarAlt;
EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
But as soon as I enable BARO mode, the hexacopter goes ballistic regardless of the alt being < 300 or > 300. The altitude read from wireless telemetry is spot on across the whole range from the ground to 10+m so I cannot see how feeding BaroAlt the sonarAlt value would cause the max throttle condition when the only difference is that the sonar is more accurate at < 300cm. Even stranger when only the original BaroAlt value is being used when I am sitting at 10m, and it still wants to go ballistic. If I comment out the extra if test, the baro alt hold goes back to normal. I thought the issue might be that sonarAlt is int16_t, however EstAlt is showing the correct value and the debug is showing the sonarAlt correctly.
Has anyone managed to meld sonar values into this offshoot of the 1311 code? I know there was talk about using sonar on the last few meters of a RTH auto landing.