How to use RPI3 as a companion device between pixhawk(PX4,APM) and ground station,with Dronee Pilot.

Below topics cover how to use RPI3 as repeater, as companion device and both.

First thing in RPI3 is to enable serial port for the the transmitter. Pixhawk is shown in /dev/ttyACM0 serial port, and the transmitter is in /dev/ttyS0.

Most of the linux systems favor buffering data to optimize throughput as same as in Raspbian,but in this case we need low latency transmission/handling. First of all we need to tell to the linux to threat with both pixhawk and transmitter as low latency devices.


setserial /dev/ttyACM0 low_latency
setserial /dev/S0 low_latency
 

PX4 connects with 115200 of baud rate by default,make sure that you set the transmitter's baudrate 115200 or above.

Using RPI3 as repeater.

The easiest way is to use socat,socat is upgraded version of netcat which comes default in all linux with many functionality and options. To install socat:


sudo apt-get install socat
 
Below command connects pixhawk and transmitter in 2way communication with low latency enabled and baud rate of 115200.

socat /dev/ttyACM0,nonblock,raw,echo=0,waitlock=/var/run/ttyLock,min=0,b115200 /dev/ttyS0,nonblock,raw,echo=0,waitlock=/var/run/tty,min=0,b115200
  
Another is to write a simple repeater code as below code in github repeats data in both directions in non blocking mode.

  static SerialPort sp1(port1,baudRate1),sp2(port2,baudRate2);
  sp1.open_serial();
  sp2.open_serial();
  static char buffer1[BUFFER_LENGTH],buffer2[BUFFER_LENGTH];
  thread([](){
  	while(true){
    	int l = sp2._read_port(buffer2,BUFFER_LENGTH);
        if(l>0){
	   sp1._write_port(buffer2, l);
        }
           usleep(5*1000);
          }
   }).detach();
        
        
  while(true){
  	int l = sp1._read_port(buffer1,BUFFER_LENGTH);
    if(l>0){
    	sp2._write_port(buffer1, l);
   	 }
     usleep(5*1000);         
 }
 
To clone from the github

git clone https://github.com/maksimpiriyev/serial_port_repeater
 
compile with the below command

./compile
 
to run it

./repeater /dev/ttyACM0 115200 /dev/ttyS0 115200
 

Using RPI3 as companion device.

If you just want to write code that will work inside the drone. You just open the /dev/ttyACM0 serial port and parse the bytes with mavlink parser, here is the below code and github repository


 static char buffer1[BUFFER_LENGTH];
 int l = sp1._read_port(buffer1,BUFFER_LENGTH);
 if(l>0){
    static mavlink_message_t msg;
    static mavlink_status_t status;
    for (int i = 0; i < l; ++i)
    {
    	if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)){
           // Packet received
        	printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
         }
     }
  }
 
To compile:

./compile_companion
 
To run:

./companion /dev/ttyACM0
 

Using RPI3 as repeater and companion device at the same time by handling some functionality in the RPI3 drone itself.

This is is the same as above repeater but handling mavlink is also integrated. To compile:


./compile_companion_repeater
 
To run:

./repcomp /dev/ttyACM0 /dev/ttyS0


Handling mavlink messages without interfering repeating.

There are sometimes cases handlink mavlink packets can take some long amount of time,especially when you do vision processing.In order to prevent latency problems in retransmission mavlink messages should be handled in another thread.

Below is the easy readable thread safe queue gets data and push it to another thread.


template
class thread_safe_queue{
    queue q;
    mutex m;
    condition_variable cv;
public:
    K pop(){
        unique_lock l(m);
        if(!q.size()){
            cv.wait(l);
        }
        auto m = q.front();
        q.pop();
        return m;
    }
    void push(K k){
        lock_guard l(m);
        q.push(k);
        cv.notify_one();
    }
};
 

 

Here is the handling code.Note that extracting mavlink messages have deterministic reasonable time,so they can be handled in the retransmission thread.

 


 static thread_safe_queue q;
 thread([]{
         while(true){
             auto m = v.pop();
             if(m.msgid == MAVLINK_MSG_ID_HEARTBEAT){
			 	// handling heartbeat
			 }else{
			 	// some vision code
			 }
         }
     }).detach();
	 
 
 static char buffer1[BUFFER_LENGTH];
 int l = sp1._read_port(buffer1,BUFFER_LENGTH);
 if(l>0){
    static mavlink_message_t msg;
    static mavlink_status_t status;
    for (int i = 0; i < l; ++i)
    {
    	if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)){
           q.push(msg);
         }
     }
  }
 

Restarting program in possible crash.

To make the program restart itself we will use supervisor. Supervisor deosn't come built i in Raspbian. To install.


sudo apt-get install supervisor
sudo service supervisor restart
 
Then we need to generate config file as below for the example repeater app.

  [program:repeater]
  command=/home/pi/repeater /dev/ttyACM0 115200 /dev/ttyS0 115200
  autostart=true
  autorestart=true
  stderr_logfile=/var/log/repeater.err.log
  stdout_logfile=/var/log/repeater.out.log
 
Once our configuration file is created and saved, we can inform Supervisor of our new program through the supervisorctl command. First we tell Supervisor to look for any new or changed program configurations in the /etc/supervisor/conf.d directory with:

  supervisorctl reread
  
Followed by telling it to enact any changes with:

 supervisorctl update
  

Any time you make a change to any program configuration file, running the two previous commands will bring the changes into effect.

Now, you are all set!


Leave a comment