[mr_w]

Hi,

I'm working on reviving hcsr04 sonar support ( https://bitbucket.org/mindnever/librepilot/src/hcsr04 ), and so far I have working implementation that uses Sensors module framework which nicely updates SonarAltitude UAVo.

Now, what I'd like to do is to have PositionState.Down depend on SonarAltitude.Altitude when its available ( sensor connected and ground distance within supported range ) but go back to BaroSensor.Altitude otherwise.

I am not really sure what to do next:

- try to understand how StateEstimation works and add sonar sensor data there.
or
- cheat and somehow fake BaroSensor.Altitude when SonarAltitude.Altitude is available.

It seems that both cases would also need to deal with transition case when switching from sonar to baro during the flight in order to avoid false/apparent altitude jumps.

I'm seeking for some more thoughts on these issues...

Thanks

[f5soh]

Hi Vladimir,

That a good news, good job.

I added a JIRA, you should rename at least your branch with 'LP-225' in his name so JIRA can find branch attached when pushed for PR / merged.
https://librepilot.atlassian.net/browse/LP-225
Take a look at .commit-template file in source code.

Not sure how sonar.altitude can be added safely, but using filteraltitude.c

Thanks
Laurent

[TheOtherCliff]

Since a lot of code already uses "absolute altitude" the solution I would try would be to filter all baro altitude through the sonar code.  The sonar code would have an offset that it adds to the baro altitude before letting the rest of the system see the baro altitude.

Of course be aware that altitude is measured backwards, it is a down distance, not an up distance.

Given that, there are cases to consider:
1) Sonar mode switched to on or sonar comes in range (imagine this happening over perfectly flat ground):
The rest of the system should not see anything different, so baro altitude must be the same.  Hidden offset stays close to zero.  When it flies over a small rise in ground, the hidden offset must create a perceived decrease in altitude.  The perceived decrease causes "Altitude Hold" etc. to climb.
2) Now comes a mentally tricky part:
When you see the sonar move down, but not the baro, you adjust the offset to make the system baro altitude look like it is at a lower altitude.  When you see the baro move down, but not the sonar, you adjust the offset to make the system baro altitude stay the same.  When you see both the baro and sonar moving down the same amount, the offset doesn't change much.  You can see that one adds to the offset and the other subtracts from it.
3) Sonar mode switched to off or sonar goes out of range (imagine this happening over perfectly flat ground):
The whole key here is that the hidden offset is now fixed at the value it had when sonar was last active.  The system baro altitude will be a little wrong, but there will be no jumps in Altitude Hold.
4) Engaging or coming in range again:
Hidden offset starts moving again.
5) Using sonar to do altitude hold while flying down a hill side:
Sonar code essentially passes the same altitude (plus or minus a little) to the system the whole way down the hill.  It does this by changing the hidden offset the whole way.

All this is great and makes sonar and baro get along, but not GPS.  The same thing needs to be done for GPS altitude.  It also needs an offset (must be separate from baro offset).

I added this to the Jira.