Custom MAVLink messages from RPi 3 to Pixhawk (for BLE positioning system)

[shebaloo (Nicola)]

Goal: we want to send custom MAVLink messages from RPi 3 to pixhawk, through Telem2 serial port. This messages have to be used to send information to our AP_Beacon_Bluebeacon.cpp custom class to integrate our BLE positioning system, that extends AP_Beacon_Backend.cpp. We process Beacon data on the RPi and we want to give to the EKF the information about Beacon distances and positions and about vehicle position.

What we did: On the ardupiulot side, we created our custom MAVLink messages inside ardupilotmega.xml in this path…\ardupilot\modules\mavlink\message_definitions\v1.0, while on the RPi side, we added them to the ardupilotmega.xml in pymavlink, we ran setup.py to generate the support code (< our_message >_encode and similar methods) to use them within the DroneKit API. On Pixhawk, we set the parameters like in this guide Communicating with Raspberry Pi via MAVLink — Dev documentation, just with the baudrate 57600 (because on RPi we set the baudrate with this value) and we have created a new value for the parameter Complete Parameter List — Copter documentation so that we enabled the use of our new .cpp class (that is correctly compiled and executed).

After these steps, we created a python script, running on RPi, sending messages like this:

msg = vehicle.message_factory.mvl_beacon_init_encode(b_x,b_y,b_z,acc)
vehicle.send_mavlink(msg)

Inside our AP_Beacon_Bluebeacon.cpp class we have

AP_HAL::UARTDriver *uart;
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MAVLink, 1);
if(mavlink_parse_char(MAVLINK_COMM_0, received, &msg, &status0)) {
if(msg.msgid == MAVLINK_MSG_ID_MVL_BEACON_INIT) {
mavlink_mvl_beacon_init_t beacon_init;
mavlink_msg_mvl_beacon_init_decode(&msg, &beacon_init);
}
}

Our problems: if we run mavproxy.py on RPi 3, we can communicate (for example, if we type “param show ARMING_CHECK”, we see the value of the parameter). If we run our python script, sending the message above, we see some MAVLink messages only on Telem1, but nothing on Telem2 (where the RPi is and should be connected).

Our questions: is there a simple way to debug the exchange of messages? Is there some configuration missing? Did we make some mistakes? Did we miss some intermediate passages?

Thanks for your help,
Nicola and team

[eMrazSVK (Eduard Mráz)]

Hello!

I did something similiar (adding new message), however I was using MAVProxy for debug.

Try to read something here, maybe it will help:
https://discuss.ardupilot.org/t/new-message-and-variables-in-apm-planner-2/11641/19

If you have any questions feel free to ask.

MAVProxy is easy to edit (when you add new messages for example to mission planner, you have to build it over and it takes long time). In mavproxy terminal, type ‘status’ to see all incoming messages.

What I see from your code, you are trying to find appropriate port of instance 1, why are you using instance 1? Did you set telem2 in any GCS to MAVLink protocol?

[shebaloo (Nicola)]

Hello,

first of all, thank you for the response!

I read what you did and it is exactly the same of what we did to generate custom MAVLink messages, we just used ardupilotmega.xml instead of common.xml.

@eMrazSVK

MAVProxy is easy to edit (when you add new messages for example to mission planner, you have to build it over and it takes long time). In mavproxy terminal, type ‘status’ to see all incoming messages.

How can we use MAVProxy to debug? How can we send our custom message from MAVProxy?

@shebaloo

uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MAVLink, 1);

This is the line of code that is refernced to your question about instance 1. We actually don’t know which is the right way to initialize the uart variable and we use that method because we saw it in other parts of code. How can we set that variable?

@eMrazSVK

Did you set telem2 in any GCS to MAVLink protocol?

Yes, I set SERIAL2_PROTOCOL to 1.

Thank you very much,
Nicola

[eMrazSVK (Eduard Mráz)]

Hi again,

Now I get it sorry, I misunderstood it. I thought you were doing it the other way (Pixhawk to RPi). So MAVProxy is useless in this case I guess. It could be used only if you were sending messages to som GCS… Ok nevermind.

So you use dronekit for sending messages. Did you used common.py somewhere? Now i am just guessing, because I have never done this.

Other thing that comes to my mind is that instance.
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MAVLink, 1);

This would be the problem for me. The second parameter in find_serial function is instance, it is counted from zero and it is used when you want to use MAVLink protocol on multiple serial ports, for example instance 0 on telem1, instance 1 on telem2 etc. Where do you use it actually? (I mean your uart variable). I do not think this is the right way to do it. Because MAVLink is implemented and it is working, that means MAVLink as a protocol is able to handle uarts (initializing them etc). Look for it somewhere.

One thing that Im not sure about, is how to debug it, maybe I ll take a look on that one later.