mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 14:29:07 +08:00
WIP state on getting MAVLink as a class, much of the work done, but does not compile yet
This commit is contained in:
Lorenz Meier
parent
03c543aba6
commit
ac77fe9c27
@ -85,42 +85,6 @@ mavlink_system_t mavlink_system = {
|
||||
0
|
||||
}; // System ID, 1-255, Component/Subsystem ID, 1-255
|
||||
|
||||
/*
|
||||
* Internal function to send the bytes through the right serial port
|
||||
*/
|
||||
void
|
||||
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
|
||||
{
|
||||
int uart = -1;
|
||||
|
||||
switch (channel) {
|
||||
case MAVLINK_COMM_0:
|
||||
uart = Mavlink::get_uart_fd(0);
|
||||
break;
|
||||
case MAVLINK_COMM_1:
|
||||
uart = Mavlink::get_uart_fd(1);
|
||||
break;
|
||||
case MAVLINK_COMM_2:
|
||||
uart = Mavlink::get_uart_fd(2);
|
||||
break;
|
||||
case MAVLINK_COMM_3:
|
||||
uart = Mavlink::get_uart_fd(3);
|
||||
break;
|
||||
case MAVLINK_COMM_4:
|
||||
uart = Mavlink::get_uart_fd(4);
|
||||
break;
|
||||
case MAVLINK_COMM_5:
|
||||
uart = Mavlink::get_uart_fd(5);
|
||||
break;
|
||||
case MAVLINK_COMM_6:
|
||||
uart = Mavlink::get_uart_fd(6);
|
||||
break;
|
||||
}
|
||||
|
||||
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* Internal function to give access to the channel status for each channel
|
||||
*/
|
||||
|
||||
@ -47,6 +47,7 @@
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <termios.h>
|
||||
#include <time.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <drivers/device/device.h>
|
||||
@ -70,10 +71,10 @@
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <commander/px4_custom_mode.h>
|
||||
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include "mavlink_parameters.h"
|
||||
#include <uORB/uORB.h>
|
||||
#include "math.h" /* isinf / isnan checks */
|
||||
#include <assert.h>
|
||||
@ -87,7 +88,9 @@
|
||||
#include <systemlib/err.h>
|
||||
#include <sys/stat.h>
|
||||
#include <dataman/dataman.h>
|
||||
#include <waypoints.h>
|
||||
#include "mavlink_main.h"
|
||||
#include "mavlink_orb_listener.h"
|
||||
#include "mavlink_receiver.h"
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
@ -102,7 +105,49 @@ static const int ERROR = -1;
|
||||
*/
|
||||
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
||||
/*
|
||||
* Internal function to send the bytes through the right serial port
|
||||
*/
|
||||
void
|
||||
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
|
||||
{
|
||||
int uart = -1;
|
||||
|
||||
switch (channel) {
|
||||
case MAVLINK_COMM_0:
|
||||
uart = Mavlink::get_uart_fd(0);
|
||||
break;
|
||||
case MAVLINK_COMM_1:
|
||||
uart = Mavlink::get_uart_fd(1);
|
||||
break;
|
||||
case MAVLINK_COMM_2:
|
||||
uart = Mavlink::get_uart_fd(2);
|
||||
break;
|
||||
case MAVLINK_COMM_3:
|
||||
uart = Mavlink::get_uart_fd(3);
|
||||
break;
|
||||
#ifdef MAVLINK_COMM_4
|
||||
case MAVLINK_COMM_4:
|
||||
uart = Mavlink::get_uart_fd(4);
|
||||
break;
|
||||
#endif
|
||||
#ifdef MAVLINK_COMM_5
|
||||
case MAVLINK_COMM_5:
|
||||
uart = Mavlink::get_uart_fd(5);
|
||||
break;
|
||||
#endif
|
||||
#ifdef MAVLINK_COMM_6
|
||||
case MAVLINK_COMM_6:
|
||||
uart = Mavlink::get_uart_fd(6);
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
|
||||
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
|
||||
|
||||
}
|
||||
|
||||
static void usage(void);
|
||||
|
||||
namespace mavlink
|
||||
{
|
||||
@ -122,6 +167,8 @@ Mavlink::Mavlink(const char *port, unsigned baud_rate) :
|
||||
_mavlink_hil_enabled(false)
|
||||
// _params_sub(-1)
|
||||
{
|
||||
wpm = &wpm_s;
|
||||
fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
|
||||
// _parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
|
||||
|
||||
}
|
||||
@ -191,6 +238,15 @@ Mavlink* Mavlink::get_instance(unsigned instance)
|
||||
return inst;
|
||||
}
|
||||
|
||||
int Mavlink::get_uart_fd(unsigned index)
|
||||
{
|
||||
Mavlink* inst = get_instance(index);
|
||||
if (inst)
|
||||
return inst->_mavlink_fd;
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::parameters_update()
|
||||
{
|
||||
@ -202,48 +258,12 @@ Mavlink::parameters_update()
|
||||
|
||||
}
|
||||
|
||||
/* XXX not widely used */
|
||||
uint8_t chan = MAVLINK_COMM_0;
|
||||
|
||||
/* XXX probably should be in a header... */
|
||||
extern pthread_t receive_start(int uart);
|
||||
|
||||
/* Allocate storage space for waypoints */
|
||||
static mavlink_wpm_storage wpm_s;
|
||||
mavlink_wpm_storage *wpm = &wpm_s;
|
||||
|
||||
bool mavlink_hil_enabled = false;
|
||||
|
||||
/* protocol interface */
|
||||
static int uart;
|
||||
static int baudrate;
|
||||
bool gcs_link = true;
|
||||
|
||||
/* interface mode */
|
||||
static enum {
|
||||
MAVLINK_INTERFACE_MODE_OFFBOARD,
|
||||
MAVLINK_INTERFACE_MODE_ONBOARD
|
||||
} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
|
||||
|
||||
static struct mavlink_logbuffer lb;
|
||||
|
||||
static void mavlink_update_system(void);
|
||||
static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
||||
static void usage(void);
|
||||
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
|
||||
|
||||
/****************************************************************************
|
||||
* MAVLink text message logger
|
||||
****************************************************************************/
|
||||
|
||||
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
|
||||
|
||||
static const struct file_operations mavlink_fops = {
|
||||
.ioctl = mavlink_dev_ioctl
|
||||
};
|
||||
|
||||
static int
|
||||
mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
|
||||
int
|
||||
Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
|
||||
{
|
||||
static unsigned int total_counter = 0;
|
||||
|
||||
@ -252,10 +272,11 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
|
||||
case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
|
||||
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
|
||||
const char *txt = (const char *)arg;
|
||||
struct mavlink_logmessage msg;
|
||||
strncpy(msg.text, txt, sizeof(msg.text));
|
||||
mavlink_logbuffer_write(&lb, &msg);
|
||||
total_counter++;
|
||||
printf("logmsg: %s\n", txt);
|
||||
//struct mavlink_logmessage msg;
|
||||
//strncpy(msg.text, txt, sizeof(msg.text));
|
||||
//mavlink_logbuffer_write(&lb, &msg);
|
||||
//total_counter++;
|
||||
return OK;
|
||||
}
|
||||
|
||||
@ -264,7 +285,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_update_system(void)
|
||||
void Mavlink::mavlink_update_system(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
static param_t param_system_id;
|
||||
@ -301,7 +322,7 @@ void mavlink_update_system(void)
|
||||
}
|
||||
}
|
||||
|
||||
int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
|
||||
int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
@ -398,7 +419,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
|
||||
}
|
||||
|
||||
int
|
||||
set_hil_on_off(bool hil_enabled)
|
||||
Mavlink::set_hil_on_off(bool hil_enabled)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
@ -427,13 +448,13 @@ set_hil_on_off(bool hil_enabled)
|
||||
hil_rate_interval = 5;
|
||||
}
|
||||
|
||||
orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
|
||||
orb_set_interval(subs.spa_sub, hil_rate_interval);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
|
||||
}
|
||||
|
||||
if (!hil_enabled && mavlink_hil_enabled) {
|
||||
mavlink_hil_enabled = false;
|
||||
orb_set_interval(mavlink_subs.spa_sub, 200);
|
||||
orb_set_interval(subs.spa_sub, 200);
|
||||
|
||||
} else {
|
||||
ret = ERROR;
|
||||
@ -443,7 +464,7 @@ set_hil_on_off(bool hil_enabled)
|
||||
}
|
||||
|
||||
void
|
||||
get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
|
||||
Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
|
||||
{
|
||||
/* reset MAVLink mode bitfield */
|
||||
*mavlink_base_mode = 0;
|
||||
@ -518,7 +539,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
|
||||
}
|
||||
|
||||
|
||||
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
|
||||
int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
@ -552,7 +573,7 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
|
||||
orb_set_interval(subs->actuators_sub, min_interval);
|
||||
orb_set_interval(subs->actuators_effective_sub, min_interval);
|
||||
orb_set_interval(subs->spa_sub, min_interval);
|
||||
orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
|
||||
orb_set_interval(subs->rates_setpoint_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
||||
@ -575,34 +596,19 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
|
||||
|
||||
extern mavlink_system_t mavlink_system;
|
||||
|
||||
extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
|
||||
extern int mavlink_missionlib_send_gcs_string(const char *string);
|
||||
|
||||
/**
|
||||
* If the queue index is not at 0, the queue sending
|
||||
* logic will send parameters from the current index
|
||||
* to len - 1, the end of the param list.
|
||||
*/
|
||||
static unsigned int mavlink_param_queue_index = 0;
|
||||
|
||||
/**
|
||||
* Callback for param interface.
|
||||
*/
|
||||
void mavlink_pm_callback(void *arg, param_t param);
|
||||
|
||||
void mavlink_pm_callback(void *arg, param_t param)
|
||||
void Mavlink::mavlink_pm_callback(void *arg, param_t param)
|
||||
{
|
||||
mavlink_pm_send_param(param);
|
||||
//mavlink_pm_send_param(param);
|
||||
usleep(*(unsigned int *)arg);
|
||||
}
|
||||
|
||||
void mavlink_pm_send_all_params(unsigned int delay)
|
||||
void Mavlink::mavlink_pm_send_all_params(unsigned int delay)
|
||||
{
|
||||
unsigned int dbuf = delay;
|
||||
param_foreach(&mavlink_pm_callback, &dbuf, false);
|
||||
}
|
||||
|
||||
int mavlink_pm_queued_send()
|
||||
int Mavlink::mavlink_pm_queued_send()
|
||||
{
|
||||
if (mavlink_param_queue_index < param_count()) {
|
||||
mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
|
||||
@ -614,22 +620,22 @@ int mavlink_pm_queued_send()
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_pm_start_queued_send()
|
||||
void Mavlink::mavlink_pm_start_queued_send()
|
||||
{
|
||||
mavlink_param_queue_index = 0;
|
||||
}
|
||||
|
||||
int mavlink_pm_send_param_for_index(uint16_t index)
|
||||
int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
|
||||
{
|
||||
return mavlink_pm_send_param(param_for_index(index));
|
||||
}
|
||||
|
||||
int mavlink_pm_send_param_for_name(const char *name)
|
||||
int Mavlink::mavlink_pm_send_param_for_name(const char *name)
|
||||
{
|
||||
return mavlink_pm_send_param(param_find(name));
|
||||
}
|
||||
|
||||
int mavlink_pm_send_param(param_t param)
|
||||
int Mavlink::mavlink_pm_send_param(param_t param)
|
||||
{
|
||||
if (param == PARAM_INVALID) return 1;
|
||||
|
||||
@ -679,11 +685,11 @@ int mavlink_pm_send_param(param_t param)
|
||||
mavlink_type,
|
||||
param_count(),
|
||||
param_get_index(param));
|
||||
ret = mavlink_missionlib_send_message(&tx_msg);
|
||||
return ret;
|
||||
mavlink_missionlib_send_message(&tx_msg);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
|
||||
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
|
||||
@ -748,7 +754,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
|
||||
}
|
||||
}
|
||||
|
||||
void publish_mission()
|
||||
void Mavlink::publish_mission()
|
||||
{
|
||||
/* Initialize mission publication if necessary */
|
||||
if (mission_pub < 0) {
|
||||
@ -759,7 +765,7 @@ void publish_mission()
|
||||
}
|
||||
}
|
||||
|
||||
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
|
||||
int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
|
||||
{
|
||||
/* only support global waypoints for now */
|
||||
switch (mavlink_mission_item->frame) {
|
||||
@ -797,7 +803,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
|
||||
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
|
||||
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
|
||||
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
|
||||
mission_item->nav_cmd = mavlink_mission_item->command;
|
||||
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
|
||||
|
||||
mission_item->time_inside = mavlink_mission_item->param1;
|
||||
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
||||
@ -807,7 +813,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
|
||||
return OK;
|
||||
}
|
||||
|
||||
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
|
||||
int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
|
||||
{
|
||||
if (mission_item->altitude_is_relative) {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
|
||||
@ -838,7 +844,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
|
||||
return OK;
|
||||
}
|
||||
|
||||
void mavlink_wpm_init(mavlink_wpm_storage *state)
|
||||
void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
|
||||
{
|
||||
state->size = 0;
|
||||
state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
|
||||
@ -854,7 +860,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
|
||||
/*
|
||||
* @brief Sends an waypoint ack message
|
||||
*/
|
||||
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
|
||||
void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
mavlink_mission_ack_t wpa;
|
||||
@ -878,7 +884,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
|
||||
*
|
||||
* @param seq The waypoint sequence number the MAV should fly to.
|
||||
*/
|
||||
void mavlink_wpm_send_waypoint_current(uint16_t seq)
|
||||
void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
|
||||
{
|
||||
if (seq < wpm->size) {
|
||||
mavlink_message_t msg;
|
||||
@ -897,7 +903,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq)
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
|
||||
void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
mavlink_mission_count_t wpc;
|
||||
@ -912,7 +918,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
|
||||
if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
|
||||
}
|
||||
|
||||
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
|
||||
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
|
||||
{
|
||||
|
||||
struct mission_item_s mission_item;
|
||||
@ -946,7 +952,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
|
||||
void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
|
||||
{
|
||||
if (seq < wpm->max_size) {
|
||||
mavlink_message_t msg;
|
||||
@ -972,7 +978,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s
|
||||
*
|
||||
* @param seq The waypoint sequence number the MAV has reached.
|
||||
*/
|
||||
void mavlink_wpm_send_waypoint_reached(uint16_t seq)
|
||||
void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
mavlink_mission_item_reached_t wp_reached;
|
||||
@ -986,7 +992,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
|
||||
}
|
||||
|
||||
|
||||
void mavlink_waypoint_eventloop(uint64_t now)
|
||||
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
|
||||
{
|
||||
/* check for timed-out operations */
|
||||
if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
|
||||
@ -1002,7 +1008,7 @@ void mavlink_waypoint_eventloop(uint64_t now)
|
||||
}
|
||||
|
||||
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
||||
void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
||||
{
|
||||
uint64_t now = hrt_absolute_time();
|
||||
|
||||
@ -1377,7 +1383,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg)
|
||||
}
|
||||
|
||||
void
|
||||
mavlink_missionlib_send_message(mavlink_message_t *msg)
|
||||
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
|
||||
{
|
||||
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
|
||||
|
||||
@ -1387,7 +1393,7 @@ mavlink_missionlib_send_message(mavlink_message_t *msg)
|
||||
|
||||
|
||||
int
|
||||
mavlink_missionlib_send_gcs_string(const char *string)
|
||||
Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
|
||||
{
|
||||
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
|
||||
mavlink_statustext_t statustext;
|
||||
@ -1419,11 +1425,11 @@ mavlink_missionlib_send_gcs_string(const char *string)
|
||||
void
|
||||
Mavlink::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
mavlink::g_mavlink->task_main();
|
||||
mavlink::g_mavlink->task_main(argc, argv);
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::task_main()
|
||||
int
|
||||
Mavlink::task_main(int argc, char *argv[])
|
||||
{
|
||||
/* inform about start */
|
||||
warnx("Initializing..");
|
||||
@ -1529,16 +1535,18 @@ Mavlink::task_main()
|
||||
err(1, "could not open %s", device_name);
|
||||
|
||||
/* create the device node that's used for sending text log messages, etc. */
|
||||
register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
|
||||
register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
|
||||
|
||||
/* Initialize system properties */
|
||||
mavlink_update_system();
|
||||
|
||||
/* start the MAVLink receiver */
|
||||
receive_thread = receive_start(uart);
|
||||
MavlinkReceiver rcv(this);
|
||||
receive_thread = rcv.receive_start(uart);
|
||||
|
||||
/* start the ORB receiver */
|
||||
uorb_receive_thread = uorb_receive_start();
|
||||
MavlinkOrbListener listener(this);
|
||||
uorb_receive_thread = listener.uorb_receive_start();
|
||||
|
||||
/* initialize waypoint manager */
|
||||
mavlink_wpm_init(wpm);
|
||||
@ -1546,57 +1554,57 @@ Mavlink::task_main()
|
||||
/* all subscriptions are now active, set up initial guess about rate limits */
|
||||
if (baudrate >= 230400) {
|
||||
/* 200 Hz / 5 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20);
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30);
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
|
||||
/* 10 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
|
||||
/* 10 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
|
||||
|
||||
} else if (baudrate >= 115200) {
|
||||
/* 20 Hz / 50 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
|
||||
} else if (baudrate >= 57600) {
|
||||
/* 10 Hz / 100 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
|
||||
/* 10 Hz / 100 ms ATTITUDE */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
|
||||
|
||||
} else {
|
||||
/* very low baud rate, limit to 1 Hz / 1000 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
|
||||
/* 1 Hz / 1000 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
|
||||
/* 0.5 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
|
||||
/* 0.1 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
|
||||
set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
|
||||
}
|
||||
|
||||
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
|
||||
|
||||
@ -73,6 +73,70 @@
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
|
||||
|
||||
#include <nuttx/fs/fs.h>
|
||||
|
||||
// FIXME XXX - TO BE MOVED TO XML
|
||||
enum MAVLINK_WPM_STATES {
|
||||
MAVLINK_WPM_STATE_IDLE = 0,
|
||||
MAVLINK_WPM_STATE_SENDLIST,
|
||||
MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
|
||||
MAVLINK_WPM_STATE_GETLIST,
|
||||
MAVLINK_WPM_STATE_GETLIST_GETWPS,
|
||||
MAVLINK_WPM_STATE_GETLIST_GOTALL,
|
||||
MAVLINK_WPM_STATE_ENUM_END
|
||||
};
|
||||
|
||||
enum MAVLINK_WPM_CODES {
|
||||
MAVLINK_WPM_CODE_OK = 0,
|
||||
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
|
||||
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
|
||||
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
|
||||
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
|
||||
MAVLINK_WPM_CODE_ENUM_END
|
||||
};
|
||||
|
||||
|
||||
/* 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_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
|
||||
{
|
||||
public:
|
||||
@ -104,6 +168,8 @@ public:
|
||||
|
||||
static Mavlink* get_instance(unsigned instance);
|
||||
|
||||
static int get_uart_fd(unsigned index);
|
||||
|
||||
struct mavlink_subscriptions {
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
@ -149,12 +215,19 @@ public:
|
||||
/** Actuator armed state */
|
||||
struct actuator_armed_s armed;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Pointer to the default cdev file operations table; useful for
|
||||
* registering clone devices etc.
|
||||
*/
|
||||
struct file_operations fops;
|
||||
int _mavlink_fd;
|
||||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
int _mavlink_task; /**< task handle for sensor task */
|
||||
|
||||
int _mavlink_fd;
|
||||
int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
@ -165,10 +238,45 @@ private:
|
||||
|
||||
int _params_sub;
|
||||
|
||||
orb_advert_t mission_pub = -1;
|
||||
struct mission_s mission;
|
||||
orb_advert_t mission_pub;
|
||||
struct mission_s mission;
|
||||
uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
|
||||
bool thread_running;
|
||||
bool thread_should_exit;
|
||||
|
||||
uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
|
||||
uint8_t mavlink_wpm_comp_id;
|
||||
mavlink_channel_t chan;
|
||||
|
||||
// XXX probably should be in a header...
|
||||
// extern pthread_t receive_start(int uart);
|
||||
|
||||
pthread_t receive_thread;
|
||||
pthread_t uorb_receive_thread;
|
||||
|
||||
/* Allocate storage space for waypoints */
|
||||
mavlink_wpm_storage wpm_s;
|
||||
mavlink_wpm_storage *wpm;
|
||||
|
||||
bool verbose;
|
||||
bool mavlink_hil_enabled;
|
||||
int uart;
|
||||
int baudrate;
|
||||
bool gcs_link;
|
||||
/**
|
||||
* If the queue index is not at 0, the queue sending
|
||||
* logic will send parameters from the current index
|
||||
* to len - 1, the end of the param list.
|
||||
*/
|
||||
unsigned int mavlink_param_queue_index;
|
||||
|
||||
/* interface mode */
|
||||
enum {
|
||||
MAVLINK_INTERFACE_MODE_OFFBOARD,
|
||||
MAVLINK_INTERFACE_MODE_ONBOARD
|
||||
} mavlink_link_mode;
|
||||
|
||||
struct mavlink_logbuffer lb;
|
||||
bool mavlink_link_termination_allowed;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
@ -240,7 +348,38 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
|
||||
* mavlink_pm_queued_send().
|
||||
* @see mavlink_pm_queued_send()
|
||||
*/
|
||||
void mavlink_pm_start_queued_send(void);
|
||||
void mavlink_pm_start_queued_send();
|
||||
|
||||
void mavlink_update_system();
|
||||
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
|
||||
void mavlink_waypoint_eventloop(uint64_t now);
|
||||
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
|
||||
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
|
||||
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
|
||||
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
|
||||
void mavlink_wpm_send_waypoint_current(uint16_t seq);
|
||||
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type);
|
||||
void mavlink_wpm_init(mavlink_wpm_storage *state);
|
||||
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
|
||||
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
|
||||
void publish_mission();
|
||||
|
||||
void mavlink_missionlib_send_message(mavlink_message_t *msg);
|
||||
int mavlink_missionlib_send_gcs_string(const char *string);
|
||||
|
||||
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
|
||||
|
||||
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
|
||||
|
||||
void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
|
||||
|
||||
/**
|
||||
* Callback for param interface.
|
||||
*/
|
||||
static void mavlink_pm_callback(void *arg, param_t param);
|
||||
|
||||
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
@ -248,8 +387,8 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
|
||||
void task_main_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Main sensor collection task.
|
||||
* Main mavlink task.
|
||||
*/
|
||||
void task_main() __attribute__((noreturn));
|
||||
int task_main(int argc, char *argv[]) __attribute__((noreturn));
|
||||
|
||||
};
|
||||
|
||||
@ -661,8 +661,8 @@ MavlinkOrbListener::l_control_mode(const struct listener *l)
|
||||
mavlink_state);
|
||||
}
|
||||
|
||||
static void *
|
||||
uorb_receive_thread(void *arg)
|
||||
void *
|
||||
MavlinkOrbListener::uorb_receive_thread(void *arg)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
|
||||
@ -712,7 +712,7 @@ uorb_receive_thread(void *arg)
|
||||
}
|
||||
|
||||
pthread_t
|
||||
uorb_receive_start(void)
|
||||
MavlinkOrbListener::uorb_receive_start(void)
|
||||
{
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
||||
@ -55,18 +55,21 @@ public:
|
||||
*/
|
||||
~MavlinkOrbListener();
|
||||
|
||||
/**
|
||||
* Start the mavlink task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
int start();
|
||||
// *
|
||||
// * Start the mavlink task.
|
||||
// *
|
||||
// * @return OK on success.
|
||||
|
||||
// int start();
|
||||
|
||||
/**
|
||||
* Display the mavlink status.
|
||||
*/
|
||||
void status();
|
||||
|
||||
pthread_t uorb_receive_start(void);
|
||||
void * uorb_receive_thread(void *arg);
|
||||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
@ -122,8 +125,6 @@ private:
|
||||
struct vehicle_attitude_s att;
|
||||
struct airspeed_s airspeed;
|
||||
|
||||
struct mavlink_subscriptions mavlink_subs;
|
||||
|
||||
int status_sub;
|
||||
int rc_sub;
|
||||
|
||||
@ -138,6 +139,6 @@ private:
|
||||
*/
|
||||
uint64_t last_sensor_timestamp;
|
||||
|
||||
hrt_abstime last_sent_vfr = 0;
|
||||
hrt_abstime last_sent_vfr;
|
||||
|
||||
};
|
||||
|
||||
@ -76,61 +76,42 @@
|
||||
__BEGIN_DECLS
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "mavlink_parameters.h"
|
||||
// #include "waypoints.h"
|
||||
#include "mavlink_receiver.h"
|
||||
#include "mavlink_main.h"
|
||||
#include "util.h"
|
||||
|
||||
__END_DECLS
|
||||
|
||||
/* XXX should be in a header somewhere */
|
||||
extern "C" pthread_t receive_start(int uart);
|
||||
void MavlinkReceiver::MavlinkReceiver() :
|
||||
pub_hil_global_pos(-1),
|
||||
pub_hil_local_pos(-1),
|
||||
pub_hil_attitude(-1),
|
||||
pub_hil_gps(-1),
|
||||
pub_hil_sensors(-1),
|
||||
pub_hil_gyro(-1),
|
||||
pub_hil_accel(-1),
|
||||
pub_hil_mag(-1),
|
||||
pub_hil_baro(-1),
|
||||
pub_hil_airspeed(-1),
|
||||
pub_hil_battery(-1),
|
||||
hil_counter(0),
|
||||
hil_frames(0),
|
||||
old_timestamp(0),
|
||||
cmd_pub(-1),
|
||||
flow_pub(-1),
|
||||
offboard_control_sp_pub(-1),
|
||||
vicon_position_pub(-1),
|
||||
telemetry_status_pub(-1),
|
||||
lat0(0),
|
||||
lon0(0),
|
||||
alt0(0)
|
||||
{
|
||||
|
||||
static void handle_message(mavlink_message_t *msg);
|
||||
static void *receive_thread(void *arg);
|
||||
}
|
||||
|
||||
static mavlink_status_t status;
|
||||
static struct vehicle_vicon_position_s vicon_position;
|
||||
static struct vehicle_command_s vcmd;
|
||||
static struct offboard_control_setpoint_s offboard_control_sp;
|
||||
|
||||
struct vehicle_global_position_s hil_global_pos;
|
||||
struct vehicle_local_position_s hil_local_pos;
|
||||
struct vehicle_attitude_s hil_attitude;
|
||||
struct vehicle_gps_position_s hil_gps;
|
||||
struct sensor_combined_s hil_sensors;
|
||||
struct battery_status_s hil_battery_status;
|
||||
static orb_advert_t pub_hil_global_pos = -1;
|
||||
static orb_advert_t pub_hil_local_pos = -1;
|
||||
static orb_advert_t pub_hil_attitude = -1;
|
||||
static orb_advert_t pub_hil_gps = -1;
|
||||
static orb_advert_t pub_hil_sensors = -1;
|
||||
static orb_advert_t pub_hil_gyro = -1;
|
||||
static orb_advert_t pub_hil_accel = -1;
|
||||
static orb_advert_t pub_hil_mag = -1;
|
||||
static orb_advert_t pub_hil_baro = -1;
|
||||
static orb_advert_t pub_hil_airspeed = -1;
|
||||
static orb_advert_t pub_hil_battery = -1;
|
||||
|
||||
/* packet counter */
|
||||
static int hil_counter = 0;
|
||||
static int hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
static orb_advert_t cmd_pub = -1;
|
||||
static orb_advert_t flow_pub = -1;
|
||||
|
||||
static orb_advert_t offboard_control_sp_pub = -1;
|
||||
static orb_advert_t vicon_position_pub = -1;
|
||||
static orb_advert_t telemetry_status_pub = -1;
|
||||
|
||||
// variables for HIL reference position
|
||||
static int32_t lat0 = 0;
|
||||
static int32_t lon0 = 0;
|
||||
static double alt0 = 0;
|
||||
|
||||
static void
|
||||
handle_message(mavlink_message_t *msg)
|
||||
void
|
||||
MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
{
|
||||
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||
|
||||
@ -804,8 +785,8 @@ handle_message(mavlink_message_t *msg)
|
||||
/**
|
||||
* Receive data from UART.
|
||||
*/
|
||||
static void *
|
||||
receive_thread(void *arg)
|
||||
void *
|
||||
MavlinkReceiver::receive_thread(void *arg)
|
||||
{
|
||||
int uart_fd = *((int *)arg);
|
||||
|
||||
@ -851,8 +832,13 @@ receive_thread(void *arg)
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void MavlinkReceiver::print_status()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
pthread_t
|
||||
receive_start(int uart)
|
||||
MavlinkReceiver::receive_start(int uart)
|
||||
{
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
@ -40,6 +40,36 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/mission_item_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls_effective.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
||||
class Mavlink;
|
||||
|
||||
class MavlinkReceiver
|
||||
@ -65,7 +95,9 @@ public:
|
||||
/**
|
||||
* Display the mavlink status.
|
||||
*/
|
||||
void status();
|
||||
void print_status();
|
||||
|
||||
pthread_t receive_start(int uart);
|
||||
|
||||
private:
|
||||
|
||||
@ -75,14 +107,41 @@ private:
|
||||
|
||||
Mavlink* _mavlink;
|
||||
|
||||
/**
|
||||
* 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 handle_message(mavlink_message_t *msg);
|
||||
void *receive_thread(void *arg);
|
||||
|
||||
mavlink_status_t status;
|
||||
struct vehicle_vicon_position_s vicon_position;
|
||||
struct vehicle_command_s vcmd;
|
||||
struct offboard_control_setpoint_s offboard_control_sp;
|
||||
struct vehicle_global_position_s hil_global_pos;
|
||||
struct vehicle_local_position_s hil_local_pos;
|
||||
struct vehicle_attitude_s hil_attitude;
|
||||
struct vehicle_gps_position_s hil_gps;
|
||||
struct sensor_combined_s hil_sensors;
|
||||
struct battery_status_s hil_battery_status;
|
||||
orb_advert_t pub_hil_global_pos;
|
||||
orb_advert_t pub_hil_local_pos;
|
||||
orb_advert_t pub_hil_attitude;
|
||||
orb_advert_t pub_hil_gps;
|
||||
orb_advert_t pub_hil_sensors;
|
||||
orb_advert_t pub_hil_gyro;
|
||||
orb_advert_t pub_hil_accel;
|
||||
orb_advert_t pub_hil_mag;
|
||||
orb_advert_t pub_hil_baro;
|
||||
orb_advert_t pub_hil_airspeed;
|
||||
orb_advert_t pub_hil_battery;
|
||||
int hil_counter;
|
||||
int hil_frames;
|
||||
uint64_t old_timestamp;
|
||||
orb_advert_t cmd_pub;
|
||||
orb_advert_t flow_pub;
|
||||
orb_advert_t offboard_control_sp_pub;
|
||||
orb_advert_t vicon_position_pub;
|
||||
orb_advert_t telemetry_status_pub;
|
||||
int32_t lat0;
|
||||
int32_t lon0;
|
||||
double alt0;
|
||||
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user