Servers by jDrones

How do you write code to send and receive MAVLink messages from a custom library script?

Hi,

I am currently working on a project using the latest Arduplane 4.0 codebase that requires me to make a custom script that will send and receive MAVLink messages from the mRo Pixhawk to a DroneKit script. I am starting by trying to send the heartbeat message from the Pixhawk to the DroneKit script. I made the DroneKit script (shown below) and tested it on the standard ArduPlane 4.0. It was able to successfully receive the heartbeat messages being sent by the code at 1Hz, so I know that part of my project works.

from __future__ import print_function

from dronekit import connect, VehicleMode, Vehicle
from pymavlink import mavutil
import time

#Set up option parsing to get connection string
import argparse  
parser = argparse.ArgumentParser(description='Reads the heartbeat of a vehicle.')
parser.add_argument('--connect',required=True,help="vehicle connection target.")
args = parser.parse_args()
connection_string = args.connect

# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)

print('Display HEARTBEAT messages for 5 seconds and then exit.')

#Create a message listener
@vehicle.on_message('HEARTBEAT')
def heartbeat_listener(self, name, message):
    print("message: ", message)


time.sleep(5)

#Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()

I have since been working on writing the custom script to send MAVLink messages, but I haven’t been able to get them to work. I have gone through several differenct code iterations, one of which is shown below, including setting up a GCS_Dummy object and then calling the gcs().send_message(MSG_HEARTBEAT); at a 1 hz rate from the scheduler. When I run the DroneKit script however, no heartbeat is found on connection to the vehicle. Does anyone have any suggestions as to how I need to set up the MAVLink communication or can point out what I am doing wrong?

I have since been working on writing the custom C++ script for the Pixhawk to send MAVLink messages to the computer running the above DroneKit script, but I haven’t been able to get the Ardupilot code to work. I have gone through several different code iterations, one of which is shown below, where I set up a GCS_Dummy object and then call the gcs().send_message(MSG_HEARTBEAT); command at a 1 hz rate from the scheduler. When I run the DroneKit script however, no heartbeat is found on connection to the vehicle. Does anyone have any suggestions as to how I need to set up the MAVLink communication or can point out what I am doing wrong?

/*
    Streamer code for Pixhawk
    Samuel Deal
    1/14/20

    This simple script will send mavlink messages to DroneKit and then receive messages back
*/

#include <AP_AHRS/AP_AHRS.h>
#include <AP_Arming/AP_Arming.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <GCS_MAVLink/GCS_Dummy.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_Param/AP_Param.h>
#include <AP_IOMCU/AP_IOMCU.h>
#include <AP_Filesystem/posix_compat.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/ap_message.h>

// hardware abstraction object for accessing many of the board specific functions
// feature ultimately makes the sofware portable to many different boards
const AP_HAL::HAL& hal = AP_HAL::get_HAL();

extern mavlink_system_t mavlink_system;

const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
    AP_GROUPEND
};

static MAVLink_routing routing;

class Streamer {
public:
    void setup();
    void loop();

private:

    // create objects for the inertial sensors
    AP_InertialSensor ins;
    AP_AHRS_DCM ahrs;
    //GCS *gcs;
    // GCS selection
    GCS_Dummy _gcs; // avoid using this; use gcs()
    GCS_Dummy &gcs() { return _gcs; }

    // define a variable for the serial terminal uart
    AP_HAL::UARTDriver *uart = hal.console;   // pixhawk1 usb
    //AP_HAL::UARTDriver *uart = hal.uartC;     // pixhawk1 telem1, pixhawk4 mini telem1
    //AP_HAL::UARTDriver *uart = hal.uartG;     // pixhawk4 mini usb

    // create a scheduler object
    AP_Scheduler scheduler{nullptr};


    // scheduler object
    static const AP_Scheduler::Task scheduler_tasks[];

    // send attitude
    void send_attitude();
    // send heartbeat
    void send_heartbeat();
    // get servo position
    void get_rc_in();
};

static AP_BoardConfig board_config;
static Streamer streamer;

#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(Streamer, &streamer, func, _interval_ticks, _max_time_micros)

/*
  scheduler table - all regular tasks are listed here, along with how
  often they should be called (in 20ms units) and the maximum time
  they are expected to take (in microseconds)
 */
const AP_Scheduler::Task Streamer::scheduler_tasks[] = {
   SCHED_TASK(send_attitude,               50,     100),
    SCHED_TASK(send_heartbeat,               1,     100),
    SCHED_TASK(get_rc_in,                   50,     100),

};


void Streamer::setup()
{

    // standard board initialization
    board_config.init();
    // setup the servo output
    hal.rcout->enable_ch(1);
    // arm the servo output, no need for a safety button
    hal.rcout->force_safety_off();

    // iniitialize the sensors  
    ins.init(50);      // init the inertial sensor to 100 Hz
    ahrs.init();

    gcs().setup_console();
    gcs().setup_uarts();

    // initialise the scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), (uint32_t)-1);

}

void Streamer::loop()
{
    // run all tasks
    scheduler.loop();
}

// reads the sensor data
void Streamer::send_heartbeat()
{
    uart->printf("\n\rSending Heartbeat...\n\r\n\r");
    hal.scheduler->delay(5);
    gcs().send_message(MSG_HEARTBEAT);
    //gcs().update_send();
}

// reads the sensor data
void Streamer::send_attitude()
{
    // update and read the sensors
    ahrs.update();

    uart->printf("Sending Attitude...\n\r");
}

// high frequency control function
void Streamer::get_rc_in()
{
    uart->printf("Get Servo Command...\n\r");
}


/*
  compatibility with old pde style build
 */
void setup(void);
void loop(void);

void setup(void)
{
    streamer.setup();
}
void loop(void)
{
    streamer.loop();
}
AP_HAL_MAIN();    /*
    Streamer code for Pixhawk
    Samuel Deal
    1/14/20

    This simple script will send mavlink messages to DroneKit and then receive messages back
*/

#include <AP_AHRS/AP_AHRS.h>
#include <AP_Arming/AP_Arming.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <GCS_MAVLink/GCS_Dummy.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_Param/AP_Param.h>
#include <AP_IOMCU/AP_IOMCU.h>
#include <AP_Filesystem/posix_compat.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/ap_message.h>

// hardware abstraction object for accessing many of the board specific functions
// feature ultimately makes the sofware portable to many different boards
const AP_HAL::HAL& hal = AP_HAL::get_HAL();

extern mavlink_system_t mavlink_system;

const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
    AP_GROUPEND
};

static MAVLink_routing routing;

class Streamer {
public:
    void setup();
    void loop();

private:

    // create objects for the inertial sensors
    AP_InertialSensor ins;
    AP_AHRS_DCM ahrs;
    //GCS *gcs;
    // GCS selection
    GCS_Dummy _gcs; // avoid using this; use gcs()
    GCS_Dummy &gcs() { return _gcs; }

    // define a variable for the serial terminal uart
    AP_HAL::UARTDriver *uart = hal.console;   // pixhawk1 usb
    //AP_HAL::UARTDriver *uart = hal.uartC;     // pixhawk1 telem1, pixhawk4 mini telem1
    //AP_HAL::UARTDriver *uart = hal.uartG;     // pixhawk4 mini usb

    // create a scheduler object
    AP_Scheduler scheduler{nullptr};


    // scheduler object
    static const AP_Scheduler::Task scheduler_tasks[];

    // send attitude
    void send_attitude();
    // send heartbeat
    void send_heartbeat();
    // get servo position
    void get_rc_in();
};

static AP_BoardConfig board_config;
static Streamer streamer;

#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(Streamer, &streamer, func, _interval_ticks, _max_time_micros)

/*
  scheduler table - all regular tasks are listed here, along with how
  often they should be called (in 20ms units) and the maximum time
  they are expected to take (in microseconds)
 */
const AP_Scheduler::Task Streamer::scheduler_tasks[] = {
   SCHED_TASK(send_attitude,               50,     100),
    SCHED_TASK(send_heartbeat,               1,     100),
    SCHED_TASK(get_rc_in,                   50,     100),

};


void Streamer::setup()
{

    // standard board initialization
    board_config.init();
    // setup the servo output
    hal.rcout->enable_ch(1);
    // arm the servo output, no need for a safety button
    hal.rcout->force_safety_off();

    // iniitialize the sensors  
    ins.init(50);      // init the inertial sensor to 100 Hz
    ahrs.init();

    gcs().setup_console();
    gcs().setup_uarts();

    // initialise the scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), (uint32_t)-1);

}

void Streamer::loop()
{
    // run all tasks
    scheduler.loop();
}

// reads the sensor data
void Streamer::send_heartbeat()
{
    uart->printf("\n\rSending Heartbeat...\n\r\n\r");
    hal.scheduler->delay(5);
    gcs().send_message(MSG_HEARTBEAT);
    //gcs().update_send();
}

// reads the sensor data
void Streamer::send_attitude()
{
    // update and read the sensors
    ahrs.update();

    uart->printf("Sending Attitude...\n\r");
}

// high frequency control function
void Streamer::get_rc_in()
{
    uart->printf("Get Servo Command...\n\r");
}


/*
  compatibility with old pde style build
 */
void setup(void);
void loop(void);

void setup(void)
{
    streamer.setup();
}
void loop(void)
{
    streamer.loop();
}
AP_HAL_MAIN();

2 3 / 3

Servers by jDrones