In order to read the sensor at maximum speed you would need read the serial stream directly from the sonar its self. currently there is no native serial driver but this is a serial lua driver that @Yuri_Rage created. its very fast but because the rangefinder is being read into lua but there is currently no lua binding for a rangefinder so it can only work within lua for actions and sending alerts and values it wont work with avoidance or anything like that currently.
local SCRIPT_NAME = 'Depthfinder'
local MAV_DEPTH_LABEL = 'DEPTH'
local BOOT_DELAY_MS = 2000
local RUN_INTERVAL_MS = 50
local SERIAL_PORT = 0
local BAUD_RATE = 9600
local FRAME_SIZE = 4
local SPEED_RATIO = 4.126
local INVALID_DEPTH = -1
local MAV_SEVERITY_WARNING = 4
local MAV_SEVERITY_INFO = 6
local port = serial:find_serial(SERIAL_PORT)
port:begin(BAUD_RATE)
port:set_flow_control(0)
local function serial_flush(bytes)
for i = 0, bytes - 1 do
port:read()
end
end
function update()
local bytes_available = port:available():toint()
if bytes_available ~= FRAME_SIZE then
serial_flush(bytes_available)
gcs:send_named_float(MAV_DEPTH_LABEL, INVALID_DEPTH)
return update, RUN_INTERVAL_MS
end
local data = 0
for i = 0, bytes_available - 1 do
data = data << 8 | port:read()
end
local sum = data & 0xFF
local l_data = data >> 8 & 0xFF
local h_data = data >> 16 & 0xFF
if (0xFF + h_data + l_data) & 0xFF ~= sum then
gcs:send_text(MAV_SEVERITY_WARNING,
string.format('%s: Bad checksum', SCRIPT_NAME))
gcs:send_named_float(MAV_DEPTH_LABEL, INVALID_DEPTH)
return update, RUN_INTERVAL_MS
end
local depth_raw = h_data << 8 | l_data
--gcs:send_named_float('RAW', depth_raw)
gcs:send_named_float(MAV_DEPTH_LABEL, depth_raw * SPEED_RATIO * 0.001)
return update, RUN_INTERVAL_MS
end
gcs:send_text(MAV_SEVERITY_INFO, string.format('%s: Script Active', SCRIPT_NAME))
return update, BOOT_DELAY_MS
I have been busy, I wasnt happy with the arduino converter as it was still getting read as a Sr04 rangefinder without all the benefits of serial mode. so this is my latest creation, Using the TFmini>maxbotix i2c adapter @ppoirier made as a start I copied in the serial sonar code from an example script i found and now its working!
I have made another version of the serial i2c adapter that should work with the rangefinder in mode 1 and mode 2, so its only pinging if requested rather than every 100ms. it should give more stable returns as it will be going through all the rangefinders sequentially rather than them all pinging constantly.
latest version is fully operational with averaging and mode selection
there are photos here, the code runs on a STM32blackpill board using stmduino. you will need to lookup the pinout for the board to see what pins are for the serial ports .
I believe A0 is for the position feedback, it connects to the signal out on the servos potentiometer, A1 is for the servo PWM output and the flight controller connects to uart 1 and the sonar to uart 2.
good time of day. I’m not familiar with coding .and I use a translator so I’m sorry if it’s not clear. how to change the code to slightly negative values ( - )
I created a budget fishing boat from what was. I’m using the old one FlySky FS i6 and there is no lua script. and having received a negative, I could roughly see the relief of the bottom without much expense .
using bot ChatGPT added data output to the port monitor.and added add a minus sign to the value of the reading_cm variable ```cpp #include <Wire.h> #include “SR04.h”
Serial.print(“-”); // Print “-” before the reading_cm value
Serial.println(abs(reading_cm)); // Print the absolute value of reading_cm to serial monitor
}
void requestEvent()
{
byte sendhi;
byte sendli;
byte sendbyte[2];
uint16_t tempreading_cm = abs(reading_cm); // Get the absolute value of reading_cm
sendhi = tempreading_cm >> 8;
sendli = tempreading_cm & 0xff;
sendbyte[0] = sendhi;
sendbyte[1] = sendli;
Wire.write(sendbyte, 2);
}
Rangefinders don’t work with negative values, they only show distance to an object .
you could use a lua script to invert the value and create a virtual sensor with the value you want. I was using it to scale the rangefinder for the spee of sound in water but it would work if you want to invert it.
I tried to test the aj-sr04m in water. I lowered it from a boat into the water at different depths. The readings are always minimal (200 mm). This is without correction for the speed of ultrasound in water. If the sensor is pulled out of the water, the readings change, everything works. I tried it in two modes - the default - when we measure the duration of the Echo pulse (R19 is missing) and in the M2 mode R19 = 120 kOm). I used code examples from here: How to Communicate Waterproof Ultrasonic Sensor AJ-SR04M/JSN-SR04T with Arduino/ESP32 – Probots Blog
Doesn’t work in water, regardless of depth. Tell me what to pay attention to?
how deep was the water, the single transducer wont get readings under 1.5 meters when in water. you need a dual transducer version to get readings down to 15cm.
I understand this, I read on forums about minimum depths. And I tried it at greater depths and not in barrels, but on the lake. Apart from the minimum depth, are there any special features? Does everything work in any mode (M1, M2, …)?