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();