I am trying to increase a message rate using the REQUEST_DATA_STREAM message for a specific stream(MAV_DATA_STREAM_RAW_SENSORS) and it does not work. The message rate constantly stays the same at around 80 Hz. What is the proper way for increasing the message rate for a specific stream or message?
Is this the limitation of ArduPilot implementation? I think Raw IMU data receiving rate is much higher on Ardupilot. I know that ArduPilot limits the rate by 80% of the main loop but I think 80 Hz is not that high to reach that limitation
You are confusing the main controll loop and the mavlink send loop. Those are two different things.
The first runs at 400Hz the second at 100Hz (I think)
I’m just trying to understand your project goals to offer some sort of solution. You’ve been very vague, so it’s hard to offer alternatives.
Since it’s onboard, fast data transfer rates are certainly possible. I recently got a Lua script to run at the kind of speed you’re seeking on a Cube Orange+. If the script is very simple and only sends IMU data across a serial connection at high baud rate and small packet size, maybe that would be better than MAVLink?
That sounds interesting. Is that script works only with Cube Orange+ or can alao be used with some modification with another pixhawk controller?
I think that would be better comparing with getting IMU through MAVlink
For the data rates required, you’ll want an H7 series processor. Most all autopilots based on those should be capable. May need to carefully craft the script to ensure minimum execution time and rescheduling time.
It’s simple enough that I can probably take a few minutes to write it. Might not be today, just to be clear, and I may not have time to fully test it. But it should get you close to what you need.
Here was my “speed test” on the Cube Orange+. I was getting results around 250 microseconds between rescheduled function calls. Serial transmission will significantly slow that, but I highly doubt it will be worse than the 80Hz you’re seeing over MAVLink.
local last = micros()
local sum = 0
local samples = 0
function update()
local now = micros()
sum = sum + (now - last)
samples = samples + 1
if samples == 100 then
gcs:send_named_float('MIC', (sum / samples):tofloat())
sum = 0
samples = 0
end
last = now
return update, 0
end
return update()
And here is a script that does successfully run in SITL, but I have not tested it with an actual autopilot, nor will I try to unpack the data on the other side by writing the corresponding receiver code.
What should happen:
Gyro and accel data gets packed into 32 bit little endian IEEE floating point formatted byte streams that are sent at 460 kbaud with preceding character identifiers like “GX” for “gyro x.”
I used few logic/control structures to hopefully keep execution time to a bare minimum.
Unfortunately, we do not have Lua bindings for individual IMU data, so these are fused values that MAY be filtered (I’d have to go check the source code to confirm).
The SERIALx_PROTOCOL for the port in use must be set to 28 (scripting) at 460 kbaud.
local BAUD_RATE = uint32_t(460800)
local port = assert(serial:find_serial(0), 'Could not find Scripting Serial Port')
port:begin(BAUD_RATE)
port:set_flow_control(0)
local function send_float(id, val)
local data = ('<f'):pack(val)
port:write(id:byte(1))
port:write(id:byte(2))
port:write(data:byte(1))
port:write(data:byte(2))
port:write(data:byte(3))
port:write(data:byte(4))
end
function update()
local g = ahrs:get_gyro()
send_float('GX', g:x())
send_float('GY', g:y())
send_float('GZ', g:z())
local a = ahrs:get_accel()
send_float('AX', a:x())
send_float('AY', a:y())
send_float('AZ', a:z())
return update, 0
end
return update()