dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Mavlink: Start multiple uart listeners, HIL working

This commit is contained in:
Julian Oes 2014-02-14 13:36:59 +01:00
parent ef46cd5e90
commit 523637e0f1
6 changed files with 68 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/mavlink/mavlink_main.cpp
View File

@ -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 */

55
src/modules/mavlink/mavlink_main.h
View File

@ -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_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_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;
uint16_t size;
uint16_t max_size;
enum MAVLINK_WPM_STATES current_state;
int16_t current_wp_id; ///< Waypoint in current transmission
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint32_t timeout;
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...

12
src/modules/mavlink/mavlink_orb_listener.cpp
View File

@ -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;

23
src/modules/mavlink/mavlink_orb_listener.h
View File

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

16
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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, &param);
@ -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;

3
src/modules/mavlink/mavlink_receiver.h
View File

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