sonar max range returns zero

jgoppert commented 9 years ago

I'm coding a flow only position estimator here: https://github.com/jgoppert/Firmware/tree/flow_est_update I'm using the sonar on the flow for altitude and flow camera for position estimation.

https://github.com/jgoppert/Firmware/blob/flow_est_update/src/examples/flow_position_estimator/FlowPositionEstimator.cpp

I just tested this estimator and the position estimation looks decent, but when I go into auto it climbs continuously because the altitude estimation using the flow sonars returns 0 when the sonar hits max range. I think this is very confusing. I would expect the sonar to return max range when it doesn't get a pulse back. I'm willing to work on this, but I wanted to see if there is any logic with the current code and I don't create some incompatibility with position_estimator_inav @DrTon.

I tested the sonar and noticed that particular angles return 0 even when no object is in view. I'm not sure if this is some kind of major code glitch or a hardware problem on my end.

I'm using the flow over i2c connection and the latest firmware on master for both flow and the fmu.

LorenzMeier commented 9 years ago

0 is the value for an invalid reading. Due to the minimal range sonars have valid readings are always non-zero. We could alternatively send something like int max, but it wouldn't change any of the logic behind it. The real operational range of this particular sonar is between 0.12 and ~3 m on hard surfaces.

jgoppert commented 9 years ago

I get the zero logic, but mapping no pulse and immediate pulse return to zero makes it impossible to construct a sonar only altitude estimate. I would like to add a status code to the packet to account for nominal, no return, and min dist separately. On Dec 28, 2014 6:21 PM, "Lorenz Meier" notifications@github.com wrote:

0 is the value for an invalid reading. Due to the minimal range sonars have valid readings are always non-zero. We could alternatively send something like int max, but it wouldn't change any of the logic behind it. The real operational range of this particular sonar is between 0.12 and ~3 m on hard surfaces.

— Reply to this email directly or view it on GitHub https://github.com/PX4/Flow/issues/30#issuecomment-68224666.

jgoppert commented 9 years ago

I've taken a detailed look at #29 and flight tested the existing code. I see the merit in filtering out the outliers with a mode filter, however, after some testing I have some concerns:

If two surfaces are creating returns it can ignore one of the surfaces, which is scary if the ignored surface is closer and you are doing obstacle avoidance/ altitude control.
The mode filter is limiting our bandwidth.
The data is still very jumpy and can't be used easily for control

I think an alternative method might be to just send the raw, unfiltered sonar data to the fmu. We could write more advanced estimators on the fmu that could use the vehicle position/velocity to predict surfaces positions in the environment, kind of rudimentary 1-D SLAM. So instead of sticking data from two surfaces together, we could just attribute one grouping of data to one surface, and another grouping to another surface.

pseudo-code:

min_surface = -1
min_error = large_number

for all surfaces:
  surface_distance_predicted = dot_product(surface_position - vehicle_position, sonar_dir)
  pred_error = sonar_measurement - surface_distance_predicted 
  if (pred_error < min_error):
     min_error = pred_error
     min_surface = surface

if pred_error > tol:
  create new surface
  new surface position = vehicle_position + sonar_dir*sonar_measurement
else:
  use measurement to update surface position with kalman filter

for all surfaces:
  If surface has not been seen for n steps
      forget surface

PX4 / PX4-Flow

sonar max range returns zero #30