Python script to output MAVLink

[cagiva]

kwagyeman, did you work up a py script to output MAVLink from the OpenMV module?

[zlite]

I'm one of the MAVlink contributors. What's the use case you have in mind for this? Typically, if you want to work in MAVlink, you might be better connecting OpenMV to a Pixhawk or RaspberryPi running a native MAVLink stack such as PX4 or APM and let it do the translation.

-c

[kwagyeman]

Boom:

https://github.com/kwagyeman/openmv/blo ... _target.py

https://github.com/kwagyeman/openmv/blo ... calflow.py

No clue if they work. Just did them both today. I think I got the protocol right however.

[cagiva]

Boom:

https://github.com/kwagyeman/openmv/blo ... _target.py

https://github.com/kwagyeman/openmv/blo ... calflow.py

No clue if they work. Just did them both today. I think I got the protocol right however.

Wow thanks! I'll test them and let you know. BTW, will the scripts work with both versions the OpenMV cam (the current and the new M7)?

[kwagyeman]

Just the M7 for the AprilTags one.

Did you order the OpenMV Cam M7? If so, what name is your order under? I have a few left from the initial 40 I can ship early. We're still about 2 weeks out before the first batch of the new production run will be ready.

[cagiva]

Did you order the OpenMV Cam M7?

Yes, we did. It should be under the name Guy Power.

[zlite]

Awesome. So basically, each script turns the OpenMV board into a different MAVLink compatible sensor: optical flow and april tag, respectively.

I'll try it with a Pixhawk and see if that works.

[kwagyeman]

I don't have a PixHawk to test with so, I don't know if the scripts work. The optical flow one I think needs some work. I don't really trust the find_displacement function. Need to work on that code. The apriltag one should work great however assuming I got the MAVLink protocol right.

[patrickpoirier51]

Hello kwagyeman

Did initial setup and tests this morning with BBBMINI FC and I connected to QGroundControl using
UDP from my FC ethernet port and adjusted the Landing_Target (x-y) message so its spanning between -0.2 to 0.2 Rads.
Getting the Quad ready to fly with this setup, will post results pretty soon
See attached pics of setup & Message curves on the Ground Control Console.

[kwagyeman]

So I got the protocol message format right?

[patrickpoirier51]

Hello kwagyeman,

Well, you got pretty close to a perfect score

The distance sensor need a single integer positive value = cm current_distance uint16_t Current distance reading
Changed this:
# min(max(int(z_to_mm(tag.z_translation(), tag_size) / 10), MAV_DISTANCE_SENSOR_min_distance), MAV_DISTANCE_SENSOR_max_distance),
For this:
int(z_to_mm(tag.z_translation(), tag_size) / -10),

Getting ready for demonstration fly

[patrickpoirier51]

Just a note concerning the Tag Distance, the new values are
Min : 30 = below 30 the Tag is exceeding the FoV
Max: 200 Over 200, the reading gets inaccurate and random, so it is limiting the landing range for a single sized Tag.

[kwagyeman]

Super! Can you submit a PR for your script changes to our code base?

https://github.com/openmv/openmv/blob/m ... _target.py

[Kiwa21]

@kwagyeman Do you think the M7 has enough processing power to do April Tag tracking and Optical Flow at the same time ?
You said you were not happy with the OF code right now, do you have a timeline for enhancing this ?
Would it be possible to enable/disable with an incoming Mavlink message ? so the OpenMV is not running full speed all the time when we don't need it ?

Thanks for your answers,
By the way, I ordered 2 M7 2 weeks ago for developpement with Ardupilot, do you still have stock ? or any idea of delivery date ?
Really looking forward to add the M7 to my collection of its older brothers (V1 , V2 )

[kwagyeman]

Yes, but, optical flow would run at 10 FPS then which isn't what you want. Without AprilTags it can do about 30 FPS.

As for enhancing the optical flow code, that's next on my list of things to do after porting data matrix bar code support. That said, I'm quite busy with other things right now and it's looking like I won't be free until Mayish.

Disabling the code via a message code be done.

...

MacroFab should have produced 250 OpenMV Cams last week and will make another 250 this week. I don't have any in stock in my ordering system yet however. Anyway, shipping for you should be done before the end of the month. I'm sorry for the wait but someone bought all the chips we use making it hard to get stock...

[patrickpoirier51]

Hello Gentlemen,

Did some fly tests and I will need more experiments in order to make it fly properly.
Basically it is behaving like the Arruco Tag tracking, including the scarry zoom when it is loosing track.
I will step back to the blob tracking to confirm that I have an equivalent pattern before digging too deep.

Really like the flexibility of this new toy, easy to modify , just connect usb , do the mod, upload , reset and voilà ... and all this, done on the live quadcopter, no need to reboot (especially with this BBBMini, that generally takes 2-3 minutes)
Here is the picture of the ''optical belly'' = OpenMV , Optical Flow, Lidar Lite v3. You can see as well the ''Monster Cable'' angled microUsb that is semi-permanent, I need to find something more flexible in order not to transfert vibration.

More to come...

[kwagyeman]

Cool, let me know if you need anything.

[Kiwa21]

Hello Gentlemen,

Did some fly tests and I will need more experiments in order to make it fly properly.
Basically it is behaving like the Arruco Tag tracking, including the scarry zoom when it is loosing track.
I will step back to the blob tracking to confirm that I have an equivalent pattern before digging too deep.

Really like the flexibility of this new toy, easy to modify , just connect usb , do the mod, upload , reset and voilà ... and all this, done on the live quadcopter, no need to reboot (especially with this BBBMini, that generally takes 2-3 minutes)
Here is the picture of the ''optical belly'' = OpenMV , Optical Flow, Lidar Lite v3. You can see as well the ''Monster Cable'' angled microUsb that is semi-permanent, I need to find something more flexible in order not to transfert vibration.

More to come...

What do you mean by scarry zoom ?

[patrickpoirier51]

The Quad suddenly steer to a random direction at high speed . what Fnoop describe as a '' Psychotic Wasp''.
The problem we have here is when the tracker loose tag , it aim at the latest vector that generally is a large value of x-y vector.
This problem is less intense on the Blob Tracker.. need to investigate.

[Kiwa21]

@patrickpoirier51 If you hover at a low altitude (2m) can you output a good vector to target ? Can you post here a log or txt file with the X Y vector ? Are you able to to a precision loiter over the April Tag ? (you can easily add a condition to overwrite cmb_rate at 0 so it stops descending but still does precland (horizontal_control) )

Thanks

[cglusky]

is the new mavlink msg via the zed work going to help wit any of this?

[patrickpoirier51]

Please note that I will resume testing once I receive a 2 axis Gimbal , and I am building a new Q330 for indoor testing. Should report back in a couple of days.

[patrickpoirier51]

Here we go....Another rainy weekend in the ThunderDrone

So I finally had a little time to play with OpenMV and I added the color tracking script to MavLINK

Pretty Basic actually here is the mod.:
while(True):
clock.tick()
img = sensor.snapshot()
for blob in img.find_blobs([thresholds[threshold_index]], pixels_threshold=100, area_threshold=20, merge=True):
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())
send_landing_target_packet(blob, img.width(), img.height())
print("TRACK %f %f " % (blob.cx(), blob.cy()))

and modified Mavlink Message
def send_landing_target_packet(blob, w, h):
global packet_sequence
temp = struct.pack("<qfffffbb",
0,
(((blob.cx() / w) - 0.5) * h_fov)/2.25,
(((blob.cy() / h) - 0.5) * v_fov)/1.65,
0, #int(z_to_mm(tag.z_translation(), tag_size) / -10),
0.0,
0.0,
0,
MAV_LANDING_TARGET_frame)

HERE IS THE RESULT:
https://www.youtube.com/watch?v=Z71NuJdAMMw

Actually, this is behave very correctly, its like a little dog following the red ball
This tracking is smooth and just stop at the last tracking position in case the signal is lost.
We need to implement this behavior with the tag, because the tag tracking signal is shooting an extreme vector when its losing the signal (occlusion, out of window, , too far, etc.) making the quad zoom like crazy.


This what the signals and environment looks like:

[cglusky]

great work patrick! any idea what the main difference is in the code between the color blob tracking and the apriltag tracking that causes the "zooming?"

[ash27]

Hi
What is that graph on the left? What did you plot? Was it distance from the target?
What tool did you use to scan the MAVLink and display them on the screen?

Thanks

[patrickpoirier51]

Thanks cglusky, I will test tag when I receive the gimbal.

ash27, the graph is mavlink landing target x-y signals generated by OpenMV and read from QGroundContol-Widget-Analyse, this is a great tool.
Distance is frm Lidar Lite V3 Range finder, bear in mind that I am flying indoor using PX4Flow : optical Flow

[patrickpoirier51]

Please note that this post is now on DyDrones thanks to Chris Anderson
http://diydrones.com/profiles/blogs/ope ... lored-ball

[ash27]

Can you explain me what you are doing in the code?
I am using Intel Aero. Which has build in camera. Trying to use that capability.

What are these math operations doing?

(((blob.cx() / w) - 0.5) * h_fov)/2.25,
(((blob.cy() / h) - 0.5) * v_fov)/1.65,
0, #int(z_to_mm(tag.z_translation(), tag_size) / -10),

BTW,
What is this value qffffbb? I am using dronekit-mavlink command. So I dont understand this.

temp = struct.pack("<qfffffbb",

Thanks
Ashwin

[kwagyeman]

... Intel Aero... Why does Intel have a drone...?

Anyway, um, so, how does you question relate to OpenMV? If the camera is already built-into the drone you have no need for MAVLink.

...

To answer your question:

The first thing is computing the movement vector, which has to be returned in radians or something weird, for the MAVLink protocol.

The second part is packing a struct using python. Please read up on how to serial bytes/words/longs in python using the struct module.

[rmackay9]

First off, thanks very much to @iabdalkader for writing the script to output MAVLink!

I'm one of the ArduPilot developers (www.ardupilot.org) and I would like to help integrate the OpenMV camera for use as an optical flow sensor (and later as a precision landing camera) so I have written a new AP Optical Flow driver which accepts the OPTICAL_FLOW messages that this example script sends.

I've found a couple of problems with the script which I've corrected in this modified version of the script:

- the y-axis output seems to be reversed (assuming the camera is meant to be orientated so that it's facing downwards with the camera lens towards the front of the vehicle)
- the output values seem to be too low for both x and y axis. I scaled up the X output 3.5 times and the Y output 5.3 times

Even with these changes though I'm finding that my quadcopter is not holding position well. I have not narrowed down the exact cause yet but I think it could be:

- the OPTICAL_FLOW messages may not be sent if the "displacement.response < 0.1". It would be best to send the flow rates as quickly as possible and as regularly as possible even if the camera does not see any movement
- I'm occasionally seeing large spikes in the flow data. This seems to happen when the attitude of the vehicle is far off from level but of course the camera doesn't know it's orientation so I'm unsure what's happening

Anyway, thanks very much and I hope we can get this working. I'm keen to help

EDIT: here is a graph of the vehicle's gyro values (i.e. rotation rates) and the output from the flow sensor which shows the large spikes mentioned above.

[kwagyeman]

Thanks for getting back to us on this. Can you send a PR to replace the script on our github?

https://github.com/openmv/openmv/tree/m ... 18-MAVLink?

[kwagyeman]

Note that the camera needs features below it to output a reading. Adding some filtering to it's output might help. Like, a smoothing filter and removes values that are obviously bad.

[rmackay9]

@kwagyeman, thanks for responding!

Sure, I'll raise a PR to update the script to fix the known issues (the reversal and the scaling).

I think the spikes are probably the reason the vehicle isn't holding position well (but I should investigate more to be sure). I could filter out the spikes with a "mode" filter but that will add some lag so I'd rather avoid that. The spikes seem to happen mostly when the flow begins to slow down. I don't suppose there's a chance of an issue in the flow calculation? I guess there's no known issues with the calculation that can lead to spikes?

[kwagyeman]

When the calculation confidence is low or there's a lack of features on the ground the code fails. I'm not sure how to make the output super stable. Right now, the algorithm can't handle rotation/scale, so when those happen the output is likely bad. The confidence value is supposed to help with this. Otherwise, the calculation is correct.

[rmackay9]

@kwagyeman, thanks again for the feedback.

I will see if perhaps there's some way to use the confidence value to remove some spikes. From a quick look at the ArduPilot logs it appears that sometimes it helps but often the glitch appears in the sample **after** the bad quality is reported.

So there's one reading with a low quality, then the next reading has a high quality but shows a large spike. I suppose in the OpenMV script or on the AP side we could throw away the next one or two samples if one arrives with a low quality value.

[kwagyeman]

Maybe update the message format to accept a quality value for the kalman filter in the Ardu pilot? Since the output of the camera is a noisy sensor it technically has to be filtered too.

[rmackay9]

OK thanks. Actually we've just remembered that we have a limitation on the update rate that we accept (20hz) but the sensor updates at about 42hz (on my camera anyway) so we're going to increase the maximum rate our EKF can handle.

[rmackay9]

So I've got it basically working now and have done a test flight on my regular quadcopter with an OpenMV camera mounted on the bottom providing flow values up to the flight controller. No GPS is being used in this flight.

Here is the YouTube video of it in action. Basically it works but it drfits around a bit especially when rotating. We probably shouldn't be surprised by this because kwagyeman said earlier that the current flow algorithm doesn't handle rotation well.

Anyway, decent progress, thanks to those who have helped get us this far!

[kwagyeman]

Hi, so, I had to do a lot of library fixes for random things in the last release. However, I will be fixing some things the algorithm requires to enable rotation/up/down correction and then once that is done it should be quite robust.

Please send the PR to replace the script in the main repo. Can you also update the AprilTag script too if the output format is incorrect for the AprilTag MAVLINK output.

[rmackay9]

@kwagyeman, the algo improvements sound great. I'll be happy to test them once they're available in whatever form.

I definitely own't forget to PR the changes to the script. I'm away this weekend but I should be able to PR it by next Tuesday.

I'm not familiar with the AprilTags script but I was planning to eventually test the OpenMV for use as a precision landing sensor. Let me look at my schedule a bit before making a promise on when I'll do that.

Txs!

[kwagyeman]

The AprilTag one is in the same directory as the optical flow one.

[rmackay9]

I've created a wiki page here on ardupilot.org to help users with the setup.

[kwagyeman]

Thanks, I'll make sure your script is in the next release.

[rmackay9]

Hi @kwagyeman,

I managed to test the mavlink_apriltags_landing_target.py script and produced a video of it working.

The big issue I found is that the low resolution (QQVGA, 160x120 pixels) means the maximum range is very short (a 30cm x 30cm target can only be seen at less than 3m). To be really useful I think we need to get the range to at least 10m. 20m would be even better. Simply increasing the size of the target doesn't resolve the problem because then the image becomes too big as the vehicle gets low and the final 50cm of the landing becomes uncontrolled and the vehicle lands off from the center. By the way, I've seen some people embed a small AprilTag inside the larger AprilTag but in any case, I think we need to improve the resolution somehow.

While testing I found a few issues that I have fixes for (and will PR):

- The z_to_mm() function which is used to calculate the distance to the target uses tag.z_translation() which seems to return a negative number. We need to reverse the sign so that we send a proper distance.
- The script sends the mavlink distance-sensor message but this isn't necessary because we are already sending the distance inside the landing-target message. This causes no harm but we can remove 50 lines of code to simplify things.
- The reported angles to the target are off by about 15%. I'm not sure why but I added a "fudge factor" to adjust the scaling to match the values I was seeing.

All feedback more than welcome!

[kwagyeman]

Awesome to see this working! Thanks for testing it. Please send a PR once fixed.

You'll have to embed April Tags inside of each other to get a higher resolution. This should be possible. You can make one tag the size of a pixel in the tag it's inside of.

[rmackay9]

Great thanks. I'm wondering if there is some way to increase the resolution used for AprilTags above the QQVGA resolution? When I tried to compile with QVGA I hit a 64Kb memory limit I think. Maybe the OpenMV CAM H7 has more memory?

[kwagyeman]

The H7 can do 240x240 pixels. This is the absolute max.

Alternatively, you could run the algorithm 4 times on 320x240 with a little overlap per area. It would kinda-of work.