forked from Archive/PX4-Autopilot
Mavlink: Start multiple uart listeners, HIL working
This commit is contained in:
Julian Oes
parent
ef46cd5e90
commit
523637e0f1
|
|
@ -309,6 +309,16 @@ int Mavlink::get_uart_fd(unsigned index)
|
|||
return -1;
|
||||
}
|
||||
|
||||
int Mavlink::get_uart_fd()
|
||||
{
|
||||
return _uart;
|
||||
}
|
||||
|
||||
int Mavlink::get_channel()
|
||||
{
|
||||
return (int)_chan;
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::parameters_update()
|
||||
{
|
||||
|
|
@ -945,7 +955,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
|
|||
wpa.target_component = compid;
|
||||
wpa.type = type;
|
||||
|
||||
mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
|
||||
mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
|
||||
if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
|
||||
|
|
@ -968,7 +978,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
|
|||
|
||||
wpc.seq = seq;
|
||||
|
||||
mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
|
||||
mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
|
||||
if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
|
||||
|
|
@ -988,7 +998,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
|
|||
wpc.target_component = compid;
|
||||
wpc.count = mission.count;
|
||||
|
||||
mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
|
||||
mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
|
||||
if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
|
||||
|
|
@ -1018,7 +1028,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
|
|||
wp.target_system = sysid;
|
||||
wp.target_component = compid;
|
||||
wp.seq = seq;
|
||||
mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp);
|
||||
mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
|
||||
if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
|
||||
|
|
@ -1036,7 +1046,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
|
|||
wpr.target_system = sysid;
|
||||
wpr.target_component = compid;
|
||||
wpr.seq = seq;
|
||||
mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
|
||||
mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
|
||||
if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
|
||||
|
|
@ -1061,7 +1071,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
|
|||
|
||||
wp_reached.seq = seq;
|
||||
|
||||
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
|
||||
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
|
||||
mavlink_missionlib_send_message(&msg);
|
||||
|
||||
if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
|
||||
|
|
@ -1318,7 +1328,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
|||
mavlink_mission_item_t wp;
|
||||
mavlink_msg_mission_item_decode(msg, &wp);
|
||||
|
||||
if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
|
||||
if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
|
||||
|
||||
wpm->timestamp_lastaction = now;
|
||||
|
||||
|
|
@ -1576,6 +1586,21 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
break;
|
||||
}
|
||||
|
||||
switch(_mode) {
|
||||
case MODE_OFFBOARD:
|
||||
case MODE_HIL:
|
||||
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
|
||||
break;
|
||||
case MODE_ONBOARD:
|
||||
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
|
||||
break;
|
||||
case MODE_TX_HEARTBEAT_ONLY:
|
||||
default:
|
||||
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
|
||||
warnx("Error: Unknown mode");
|
||||
break;
|
||||
}
|
||||
|
||||
/* Flush stdout in case MAVLink is about to take it over */
|
||||
fflush(stdout);
|
||||
|
||||
|
|
@ -1597,11 +1622,9 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
mavlink_update_system();
|
||||
|
||||
/* start the MAVLink receiver */
|
||||
// MavlinkReceiver rcv(this);
|
||||
receive_thread = MavlinkReceiver::receive_start(this);
|
||||
|
||||
/* start the ORB receiver */
|
||||
// MavlinkOrbListener listener(this);
|
||||
uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
|
||||
|
||||
/* initialize waypoint manager */
|
||||
|
|
|
|||
|
|
@ -96,46 +96,27 @@ enum MAVLINK_WPM_CODES {
|
|||
};
|
||||
|
||||
|
||||
/* WAYPOINT MANAGER - MISSION LIB */
|
||||
|
||||
#define MAVLINK_WPM_MAX_WP_COUNT 15
|
||||
#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
|
||||
#ifndef MAVLINK_WPM_TEXT_FEEDBACK
|
||||
#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text
|
||||
#endif
|
||||
#define MAVLINK_WPM_MAX_WP_COUNT 255
|
||||
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
|
||||
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
|
||||
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
|
||||
|
||||
|
||||
struct mavlink_wpm_storage {
|
||||
mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
|
||||
#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
|
||||
mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
|
||||
#endif
|
||||
uint16_t size;
|
||||
uint16_t max_size;
|
||||
uint16_t rcv_size;
|
||||
enum MAVLINK_WPM_STATES current_state;
|
||||
int16_t current_wp_id; ///< Waypoint in current transmission
|
||||
int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
|
||||
uint16_t current_count;
|
||||
uint8_t current_partner_sysid;
|
||||
uint8_t current_partner_compid;
|
||||
uint64_t timestamp_lastaction;
|
||||
uint64_t timestamp_last_send_setpoint;
|
||||
uint64_t timestamp_firstinside_orbit;
|
||||
uint64_t timestamp_lastoutside_orbit;
|
||||
uint32_t timeout;
|
||||
uint32_t delay_setpoint;
|
||||
float accept_range_yaw;
|
||||
float accept_range_distance;
|
||||
bool yaw_reached;
|
||||
bool pos_reached;
|
||||
bool idle;
|
||||
int current_dataman_id;
|
||||
};
|
||||
|
||||
|
||||
class Mavlink
|
||||
{
|
||||
|
||||
|
|
@ -174,7 +155,9 @@ public:
|
|||
|
||||
static int get_uart_fd(unsigned index);
|
||||
|
||||
int get_uart_fd() { return _uart; }
|
||||
int get_uart_fd();
|
||||
|
||||
int get_channel();
|
||||
|
||||
const char *device_name;
|
||||
|
||||
|
|
@ -294,7 +277,7 @@ private:
|
|||
uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
|
||||
MAVLINK_MODE _mode;
|
||||
|
||||
uint8_t mavlink_wpm_comp_id;
|
||||
uint8_t _mavlink_wpm_comp_id;
|
||||
mavlink_channel_t _chan;
|
||||
|
||||
// XXX probably should be in a header...
|
||||
|
|
|
|||
|
|
@ -55,14 +55,14 @@
|
|||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "mavlink_orb_listener.h"
|
||||
#include "mavlink_main.h"
|
||||
|
||||
void *uorb_receive_thread(void *arg);
|
||||
|
||||
|
||||
|
||||
uint16_t cm_uint16_from_m_float(float m);
|
||||
|
|
@ -647,9 +647,9 @@ void *
|
|||
MavlinkOrbListener::uorb_receive_thread(void *arg)
|
||||
{
|
||||
/* Set thread name */
|
||||
char buf[32];
|
||||
sprintf(buf, "mavlink rcv%d", Mavlink::instance_count());
|
||||
prctl(PR_SET_NAME, buf, getpid());
|
||||
char thread_name[18];
|
||||
sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel());
|
||||
prctl(PR_SET_NAME, thread_name, getpid());
|
||||
|
||||
/*
|
||||
* set up poll to block for new data,
|
||||
|
|
@ -822,7 +822,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
|
|||
pthread_attr_setstacksize(&uorb_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink);
|
||||
pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink);
|
||||
|
||||
pthread_attr_destroy(&uorb_attr);
|
||||
return thread;
|
||||
|
|
|
|||
|
|
@ -87,20 +87,7 @@ public:
|
|||
*/
|
||||
~MavlinkOrbListener();
|
||||
|
||||
// *
|
||||
// * Start the mavlink task.
|
||||
// *
|
||||
// * @return OK on success.
|
||||
|
||||
// int start();
|
||||
|
||||
/**
|
||||
* Display the mavlink status.
|
||||
*/
|
||||
void status();
|
||||
|
||||
static pthread_t uorb_receive_start(Mavlink *mavlink);
|
||||
void * uorb_receive_thread(void *arg);
|
||||
|
||||
struct listener {
|
||||
void (*callback)(const struct listener *l);
|
||||
|
|
@ -124,15 +111,7 @@ private:
|
|||
unsigned _n_listeners;
|
||||
static const unsigned _max_listeners = 32;
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
void task_main_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Main sensor collection task.
|
||||
*/
|
||||
void task_main() __attribute__((noreturn));
|
||||
void *uorb_receive_thread(void *arg);
|
||||
|
||||
static void l_sensor_combined(const struct listener *l);
|
||||
static void l_vehicle_attitude(const struct listener *l);
|
||||
|
|
|
|||
|
|
@ -798,7 +798,10 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||
|
||||
mavlink_message_t msg;
|
||||
|
||||
prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid());
|
||||
/* Set thread name */
|
||||
char thread_name[18];
|
||||
sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel());
|
||||
prctl(PR_SET_NAME, thread_name, getpid());
|
||||
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = uart_fd;
|
||||
|
|
@ -842,20 +845,19 @@ void MavlinkReceiver::print_status()
|
|||
|
||||
void * MavlinkReceiver::start_helper(void *context)
|
||||
{
|
||||
MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context));
|
||||
MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context);
|
||||
return rcv->receive_thread(NULL);
|
||||
}
|
||||
|
||||
pthread_t
|
||||
MavlinkReceiver::receive_start(Mavlink *mavlink)
|
||||
MavlinkReceiver::receive_start(Mavlink *parent)
|
||||
{
|
||||
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
// set to non-blocking read
|
||||
int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0);
|
||||
fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
|
||||
int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0);
|
||||
fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
struct sched_param param;
|
||||
(void)pthread_attr_getschedparam(&receiveloop_attr, ¶m);
|
||||
|
|
@ -865,7 +867,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink)
|
|||
pthread_attr_setstacksize(&receiveloop_attr, 3000);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink);
|
||||
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent);
|
||||
|
||||
pthread_attr_destroy(&receiveloop_attr);
|
||||
return thread;
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ public:
|
|||
*/
|
||||
void print_status();
|
||||
|
||||
static pthread_t receive_start(Mavlink* mavlink);
|
||||
static pthread_t receive_start(Mavlink *parent);
|
||||
|
||||
static void * start_helper(void *context);
|
||||
|
||||
|
|
@ -105,7 +105,6 @@ private:
|
|||
|
||||
Mavlink* _mavlink;
|
||||
|
||||
|
||||
void handle_message(mavlink_message_t *msg);
|
||||
void *receive_thread(void *arg);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue