Servers by jDrones

How does one send mavlink message from 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 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 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();

I’m perhaps a little confused, as it’s not clear to me what you’re actually trying to do: but on the surface, there should be no need to modify the Ardupilot code to do what I think you’re attempting. Just setting a serial port to Mavlink and connecting to it should be enough.
Have you looked through the DroneKit examples?

Essentially I am just trying to make a simple script to run on the Pixhawk (like the Scheduler Example) that just sends out the heartbeat mavlink message. Obviously that is not my end goal, but a very early intermediate step. Once I figure that out, I can work on sending other mavlink messages and such for my project that are more useful, but I haven’t been able to figure out how to do this simple thing. I have the DroneKit script working fine I think, I haven’t had much trouble with that. I am really just struggling to get the heartbeat message to send from the Pixhawk to the computer running DroneKit. Does that make more sense? Any ideas on what I am doing wrong would be helpful

Trying to get ardupilot to generate a second heartbeat isn’t really architecturally sound. I’d suggest a different approach.
First just get a DroneKit script connecting, and start building the guts of your final goal, but using other existing messages (and perhaps junk data).

Adding and using a new message, which I think you’re heading towards, requires adding it to the gcs_mavlink library, then adding a driver to use it. I’d suggest building it all out before adding your new message because once you do that you need to generate the new mavlink headers and recompile everything (ardupilot, pymavlink, DroneKit), and add handling for the new message in all those places.

I noticed you’d added a huge number of includes in your code. I’d suggest just starting with gcs_mavlink and those you actually need directly.

More context would be useful, so feel free to pm me if you don’t want to lay it all out in the forum.

Regards,

James

I’m not trying to add a second heartbeat to the ardupilot code. I am writing my own code from scratch using existing ardupilot functions, libraries etc. and trying to add the ability to send the heartbeat message with that code. Just like how the scheduler example doesn’t setup a second schedule loop on Ardupilot, but is its own stand-alone code that runs outside of ArduPlane or ArduCopter. I did remove a lot of the #includes recently that I wasn’t using because I agree, they weren’t necessary, but unfortunately that is not the problem with my implementation of the code

I’m still not completely clear what you want to end up with, but that seems intentional.
Have a look at GCS_Common https://github.com/ArduPilot/ardupilot/blob/14ad9a58bde667b94cfc1aae2e896cebef07ffdf/libraries/GCS_MAVLink/GCS_Common.cpp#L2289

That’s all good, maybe if you read this wiki page on library example sketches you might understand better. I’m not sure if you have ever compiled and run one of the library sketches, but basically they are a little Arduino test sketch that runs a little bit of the code (rather than the full ArduPlane code). I was just trying to make one that tests out sending MAVLink messages, and I wanted to start by sending the hearbeat message (or any message, it doesn’t really matter at the moment).

Servers by jDrones