Custom data stream from RPi to Pixhawk

ArduCopter Copter 4.2
,
1

Hi,

My goal is to send data stream from the RPi to the Pixhawk so that everything (flight log and external data) is saved to the sd card neatly in a file. For that, I created my own mavlink message definition I compiled for the RPi and Pixhawk. The RPi is running a Python script that collects the data, encodes the Mavlink message, and sends it through UART. I’m using custom pymavlink and mavutil for this.

Here is part of the RPi Python script code

from pymavlink import mavutil
mav_connection = mavutil.mavserial('/dev/serial0', baud=115200, source_system=1, source_component=191)
mav_connection.wait_heartbeat()
print("Heartbeat system: sysID %u compID %u" % (mav_connection.target_system, mav_connection.target_component)) # Output sysID 1 compID 0

mav_connection.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)

last_beat = time.time()
while(True):
	if(time.time() - last_beat > 0.95):
                            mav_connection.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
                            last_beat = time.time()

	values = [fCenterFreq, fAmplitudeDBM, 0, 0, 0]
	mav_connection.mav.sensor_raw_send(10,0,2,values,True)

I also modified the messagehandle function in ArduCopter to accept and process the external telemetry from the RPi. However, I cannot get the Pixhawk to see the custom messages. I even tried bypassing the mavlink routing within ArduCopter but the Pixhawk is not seeing it.

Here is part of what I modified in the handlemessage in GCS_Mavlink.cpp

void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
{
	.
	.
	.
	switch (msg.msgid) {

    // RFE message handle
    case MAVLINK_MSG_ID_SENSOR_RAW:
    {
        // Recieve message from RPi and handle the RFE data
        copter.MY_RFE.handle_message(chan, msg);
        // Immediately save the data to the SD card
        copter.user_RFE_logger();
        break;
    }
	.
	.
	.
}

At this point, I’m unsure if the problem is with the sender or the receiver. So I hope anyone here can guide me on how to stream data to the Pixhawk. I searched for answers all around the forums and tutorials, but they mainly describe the reverse process (data streams from the Pixhawk out).

Thanks!

2

Btw, I followed the way the ADSB-mavlink device was coded in ArduPilot. So that leads me to think that the issue may be with the RPi (the sender).

3

Hello,

A good way to debug this will be to use SITL with a debugger attached !

Make your RPI send the data to a SITL instance and look on the wiki how to use a debugger like gdb !

4

Thanks! I will try this. Speaking of debugging, I just included this piece of code in my sensor class in ArduPilot:

// search for onboard computer in GCS_MAVLink routing table
void MY_RFE::find_RPi()
{
    // return immediately if initialised
    if (_initialised) {
        return;
    }

    // return if search time has has passed
    if (AP_HAL::millis() > ARRC_RPI_SEARCH_MS) {
        gcs().send_text(MAV_SEVERITY_INFO,"No RPi found");
        return;
    }

    if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, _sysid, _compid, _chan)) {
        gcs().send_text(MAV_SEVERITY_INFO,"Found RPi sysID %d compID %d chan %d",_sysid, _compid, _chan);
        _initialised = true;
    }
}

Surprisingly, the Pixhawk reads and returns a good sysID and compID from the RPi !! I think this means the Pixhawk sees the RPi heartbeats, but it is not handling and processing the sensor message :frowning:

5

Did you find a solution? I’ve been trying to communicate with a pixhawk (4.2 rover) for a while. In the following I try to connect to SITL:

system_id = mavutil.mavlink.MAV_TYPE_GCS
port_id = "tcpin:localhost:5750"
def connect_tcp(self):  
  try:
    self._master = mavutil.mavlink_connection(self.port_id, self.system_id, self.component_id)
  except serialutil.SerialException as e:
    print(f"Non-fatal error while connecting: {e}")
    return

  # I get stuck in here. Currently letting it loop forever. I keep receiving heartbeats from SITL, but it does not seem to get my heartbeats back
  while True:
      msg = self._master.wait_heartbeat(timeout=.75)
      if msg is not None:
          print("[Vehicle:Init:SUCCESS] Vehicle connected")
      else:
          print("[Vehicle:Init] ...")
      self._master.mav.heartbeat_send(self.system_id, 
          mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)

I looked at mavproxy.py and dronekit’s github repo, and they seemingly both send heartbeats with this exact heartbeat command using mavlink 2.0. First I wanted to see what happens inside heartbeat_send(), but while I think it’s imported with pymavlink.mavutil I wasn’t able to locate the definition anywhere while searching github.

Perhaps the issue lies elsewhere? The mission planner SITL vehicle sends heartbeats, and I receive them, but what else could hinder the heartbeats in being sent to the pixhawk, if the connection is up?

I also have a serial RPi4->TELEM1/2 connection to a physical pixhawk.
The only commands I’ve been able to send (With serial connection and directly to a physical pixhawk) has been ARM, and mav.set_position_target_global_int_send() for GUIDED mode.
However the move command does not compare to an auto mission through MP, moving in a curved, zigzagged manner and stopping about ~4/5m before the waypoint if even moving at all.

6

Hi Brooke,

Sorry for the late response. Yes, I got my code to work.
The only error I see in your code is here

# I think this is wrong
self._master.mav.heartbeat_send(self.system_id, 
          mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)

# The first argument in the function has to be the mav type not the SysID. In my code above it looks like this:
mav_connection.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
7

Thanks for the response, terseven. My self.system_id is actually a MAV_TYPE (Value 6), but a good point, I was actually not aware that this was part of the MAV_TYPE enum (Despite being part of the name).

Nevertheless, I stopped attempting to make it work in SITL as I am closing on my deadline. Commands to an active vehicle seems to be alright now, with some heavy drift with time, but I suspect that may be due to the measurements, and perhaps the system not being able to catch up with my requested update frequency from the Pixhawk - a bit of tweaking here and there…!