Sonar based on HC-SR04

Blog
, ,
10

I have made a lua script for testing

the idea is to setup the depth sounder as a regular SR04 then do the scaling in lua rather than the driver.

the calculated depth should show up on the status tab of mission planner.

-- height above terrain warning script

-- mindepth, script will warn if lower than this
local water_min_depth = 2

-- warning is only enabled further than this distance from home
local home_dist_enable = 1

-- must have climbed at least this distance above terrain since arming to enable warning
local height_enable = 1

-- warning repeat time in ms
local warn_ms = 10000

--ratio between speed of sound in air and water
local speed_ratio = 4.126


local height_threshold_passed = false
local last_warn = 0
function update()
  if not arming:is_armed() then
    -- not armed, nothing to do, reset height threshold
    height_threshold_passed = false
    return update, 1000
  end




  local water_depth = rangefinder:distance_cm_orient(25) * speed_ratio (true)
  if not water_depth then
    -- could not get a valid terrain alt
    return update, 1000
  end

  if (not height_threshold_passed) and (water_depth * 0.01 < height_enable) then
    -- not climbed far enough to enable, nothing to do
    return update, 1000
  end
  height_threshold_passed = true

  local home_dist = ahrs:get_relative_position_NED_home()
  if home_dist then
    -- only check home dist if we have a valid home
    -- do not consider altitude above home in radius calc
    home_dist:z(0)
    if home_dist:length() < home_dist_enable then
      -- to close to home
      return update, 1000
    end
  end
 gcs:send_named_float('depth', water_depth)
   if (water_depth < water_min_depth) and (vehicle:get_mode() == 10) then    -- if mode is in auto 
	vehicle:set_mode(5) --set mode to loiter
    gcs:send_text(2, string.format("Depth Warning: %0.1f meters",water_depth))
    return update, warn_ms

	
  end

  return update, 1000
end

return update, 10000
1 Like