mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 11:53:34 +08:00
Cut over MAVLink to new style build system
This commit is contained in:
Lorenz Meier
@@ -0,0 +1,3 @@
|
||||
include
|
||||
mavlink-*
|
||||
pymavlink-*
|
||||
@@ -0,0 +1,825 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink.c
|
||||
* MAVLink 1.0 protocol implementation.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "util.h"
|
||||
#include "waypoints.h"
|
||||
#include "mavlink_parameters.h"
|
||||
|
||||
/* define MAVLink specific parameters */
|
||||
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
|
||||
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
|
||||
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
|
||||
|
||||
__EXPORT int mavlink_main(int argc, char *argv[]);
|
||||
|
||||
static int mavlink_thread_main(int argc, char *argv[]);
|
||||
|
||||
/* thread state */
|
||||
volatile bool thread_should_exit = false;
|
||||
static volatile bool thread_running = false;
|
||||
static int mavlink_task;
|
||||
|
||||
/* pthreads */
|
||||
static pthread_t receive_thread;
|
||||
static pthread_t uorb_receive_thread;
|
||||
|
||||
/* terminate MAVLink on user request - disabled by default */
|
||||
static bool mavlink_link_termination_allowed = false;
|
||||
|
||||
mavlink_system_t mavlink_system = {
|
||||
100,
|
||||
50,
|
||||
MAV_TYPE_FIXED_WING,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
}; // System ID, 1-255, Component/Subsystem ID, 1-255
|
||||
|
||||
/* 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);
|
||||
|
||||
|
||||
|
||||
int
|
||||
set_hil_on_off(bool hil_enabled)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
/* Enable HIL */
|
||||
if (hil_enabled && !mavlink_hil_enabled) {
|
||||
|
||||
/* Advertise topics */
|
||||
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
|
||||
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
|
||||
|
||||
/* sensore level hil */
|
||||
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
|
||||
pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
|
||||
|
||||
mavlink_hil_enabled = true;
|
||||
|
||||
/* ramp up some HIL-related subscriptions */
|
||||
unsigned hil_rate_interval;
|
||||
|
||||
if (baudrate < 19200) {
|
||||
/* 10 Hz */
|
||||
hil_rate_interval = 100;
|
||||
|
||||
} else if (baudrate < 38400) {
|
||||
/* 10 Hz */
|
||||
hil_rate_interval = 100;
|
||||
|
||||
} else if (baudrate < 115200) {
|
||||
/* 20 Hz */
|
||||
hil_rate_interval = 50;
|
||||
|
||||
} else {
|
||||
/* 200 Hz */
|
||||
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);
|
||||
}
|
||||
|
||||
if (!hil_enabled && mavlink_hil_enabled) {
|
||||
mavlink_hil_enabled = false;
|
||||
orb_set_interval(mavlink_subs.spa_sub, 200);
|
||||
|
||||
} else {
|
||||
ret = ERROR;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
|
||||
{
|
||||
/* reset MAVLink mode bitfield */
|
||||
*mavlink_mode = 0;
|
||||
|
||||
/* set mode flags independent of system state */
|
||||
|
||||
/* HIL */
|
||||
if (v_status.flag_hil_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||
}
|
||||
|
||||
/* manual input */
|
||||
if (v_status.flag_control_manual_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
}
|
||||
|
||||
/* attitude or rate control */
|
||||
if (v_status.flag_control_attitude_enabled ||
|
||||
v_status.flag_control_rates_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
}
|
||||
|
||||
/* vector control */
|
||||
if (v_status.flag_control_velocity_enabled ||
|
||||
v_status.flag_control_position_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
}
|
||||
|
||||
/* autonomous mode */
|
||||
if (v_status.state_machine == SYSTEM_STATE_AUTO) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
|
||||
}
|
||||
|
||||
/* set arming state */
|
||||
if (armed.armed) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
|
||||
} else {
|
||||
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
}
|
||||
|
||||
switch (v_status.state_machine) {
|
||||
case SYSTEM_STATE_PREFLIGHT:
|
||||
if (v_status.flag_preflight_gyro_calibration ||
|
||||
v_status.flag_preflight_mag_calibration ||
|
||||
v_status.flag_preflight_accel_calibration) {
|
||||
*mavlink_state = MAV_STATE_CALIBRATING;
|
||||
|
||||
} else {
|
||||
*mavlink_state = MAV_STATE_UNINIT;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STANDBY:
|
||||
*mavlink_state = MAV_STATE_STANDBY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_GROUND_READY:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_MANUAL:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STABILIZED:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_AUTO:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_MISSION_ABORT:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_EMCY_LANDING:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_EMCY_CUTOFF:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_GROUND_ERROR:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
*mavlink_state = MAV_STATE_POWEROFF;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
switch (mavlink_msg_id) {
|
||||
case MAVLINK_MSG_ID_SCALED_IMU:
|
||||
/* sensor sub triggers scaled IMU */
|
||||
orb_set_interval(subs->sensor_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIGHRES_IMU:
|
||||
/* sensor sub triggers highres IMU */
|
||||
orb_set_interval(subs->sensor_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_RAW_IMU:
|
||||
/* sensor sub triggers RAW IMU */
|
||||
orb_set_interval(subs->sensor_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE:
|
||||
/* attitude sub triggers attitude */
|
||||
orb_set_interval(subs->att_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
|
||||
/* actuator_outputs triggers this message */
|
||||
orb_set_interval(subs->act_0_sub, min_interval);
|
||||
orb_set_interval(subs->act_1_sub, min_interval);
|
||||
orb_set_interval(subs->act_2_sub, min_interval);
|
||||
orb_set_interval(subs->act_3_sub, min_interval);
|
||||
orb_set_interval(subs->actuators_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
||||
/* manual_control_setpoint triggers this message */
|
||||
orb_set_interval(subs->man_control_sp_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
|
||||
orb_set_interval(subs->debug_key_value, min_interval);
|
||||
break;
|
||||
|
||||
default:
|
||||
/* not found */
|
||||
ret = ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* 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)
|
||||
{
|
||||
static unsigned int total_counter = 0;
|
||||
|
||||
switch (cmd) {
|
||||
case (int)MAVLINK_IOC_SEND_TEXT_INFO:
|
||||
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++;
|
||||
return OK;
|
||||
}
|
||||
|
||||
default:
|
||||
return ENOTTY;
|
||||
}
|
||||
}
|
||||
|
||||
#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 0: speed = B0; break;
|
||||
|
||||
case 50: speed = B50; break;
|
||||
|
||||
case 75: speed = B75; break;
|
||||
|
||||
case 110: speed = B110; break;
|
||||
|
||||
case 134: speed = B134; break;
|
||||
|
||||
case 150: speed = B150; break;
|
||||
|
||||
case 200: speed = B200; break;
|
||||
|
||||
case 300: speed = B300; break;
|
||||
|
||||
case 600: speed = B600; break;
|
||||
|
||||
case 1200: speed = B1200; break;
|
||||
|
||||
case 1800: speed = B1800; break;
|
||||
|
||||
case 2400: speed = B2400; break;
|
||||
|
||||
case 4800: speed = B4800; break;
|
||||
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* open uart */
|
||||
printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
|
||||
uart = open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
*is_usb = false;
|
||||
|
||||
/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
|
||||
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else {
|
||||
*is_usb = true;
|
||||
}
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
void
|
||||
mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
|
||||
{
|
||||
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
|
||||
}
|
||||
|
||||
/*
|
||||
* Internal function to give access to the channel status for each channel
|
||||
*/
|
||||
mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
|
||||
{
|
||||
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
||||
return &m_mavlink_status[channel];
|
||||
}
|
||||
|
||||
/*
|
||||
* Internal function to give access to the channel buffer for each channel
|
||||
*/
|
||||
mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
|
||||
{
|
||||
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
|
||||
return &m_mavlink_buffer[channel];
|
||||
}
|
||||
|
||||
void mavlink_update_system(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
static param_t param_system_id;
|
||||
static param_t param_component_id;
|
||||
static param_t param_system_type;
|
||||
|
||||
if (!initialized) {
|
||||
param_system_id = param_find("MAV_SYS_ID");
|
||||
param_component_id = param_find("MAV_COMP_ID");
|
||||
param_system_type = param_find("MAV_TYPE");
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
/* update system and component id */
|
||||
int32_t system_id;
|
||||
param_get(param_system_id, &system_id);
|
||||
|
||||
if (system_id > 0 && system_id < 255) {
|
||||
mavlink_system.sysid = system_id;
|
||||
}
|
||||
|
||||
int32_t component_id;
|
||||
param_get(param_component_id, &component_id);
|
||||
|
||||
if (component_id > 0 && component_id < 255) {
|
||||
mavlink_system.compid = component_id;
|
||||
}
|
||||
|
||||
int32_t system_type;
|
||||
param_get(param_system_type, &system_type);
|
||||
|
||||
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
|
||||
mavlink_system.type = system_type;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* MAVLink Protocol main function.
|
||||
*/
|
||||
int mavlink_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* initialize mavlink text message buffering */
|
||||
mavlink_logbuffer_init(&lb, 5);
|
||||
|
||||
int ch;
|
||||
char *device_name = "/dev/ttyS1";
|
||||
baudrate = 57600;
|
||||
|
||||
/* work around some stupidity in task_create's argv handling */
|
||||
argc -= 2;
|
||||
argv += 2;
|
||||
|
||||
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'b':
|
||||
baudrate = strtoul(optarg, NULL, 10);
|
||||
|
||||
if (baudrate == 0)
|
||||
errx(1, "invalid baud rate '%s'", optarg);
|
||||
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
device_name = optarg;
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
mavlink_link_termination_allowed = true;
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
|
||||
break;
|
||||
|
||||
default:
|
||||
usage();
|
||||
}
|
||||
}
|
||||
|
||||
struct termios uart_config_original;
|
||||
|
||||
bool usb_uart;
|
||||
|
||||
/* print welcome text */
|
||||
warnx("MAVLink v1.0 serial interface starting...");
|
||||
|
||||
/* inform about mode */
|
||||
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
|
||||
|
||||
/* Flush stdout in case MAVLink is about to take it over */
|
||||
fflush(stdout);
|
||||
|
||||
/* default values for arguments */
|
||||
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
|
||||
|
||||
if (uart < 0)
|
||||
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);
|
||||
|
||||
/* Initialize system properties */
|
||||
mavlink_update_system();
|
||||
|
||||
/* start the MAVLink receiver */
|
||||
receive_thread = receive_start(uart);
|
||||
|
||||
/* start the ORB receiver */
|
||||
uorb_receive_thread = uorb_receive_start();
|
||||
|
||||
/* initialize waypoint manager */
|
||||
mavlink_wpm_init(wpm);
|
||||
|
||||
/* 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);
|
||||
/* 50 Hz / 20 ms */
|
||||
set_mavlink_interval_limit(&mavlink_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);
|
||||
/* 10 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
|
||||
/* 10 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_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);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_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);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_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);
|
||||
/* 10 Hz / 100 ms ATTITUDE */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
|
||||
/* 5 Hz / 200 ms */
|
||||
set_mavlink_interval_limit(&mavlink_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);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_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);
|
||||
/* 1 Hz / 1000 ms */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
|
||||
/* 0.5 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
|
||||
/* 0.1 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
|
||||
}
|
||||
|
||||
thread_running = true;
|
||||
|
||||
/* arm counter to go off immediately */
|
||||
unsigned lowspeed_counter = 10;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* 1 Hz */
|
||||
if (lowspeed_counter == 10) {
|
||||
mavlink_update_system();
|
||||
|
||||
/* translate the current system state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_mode = 0;
|
||||
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
|
||||
|
||||
/* switch HIL mode if required */
|
||||
set_hil_on_off(v_status.flag_hil_enabled);
|
||||
|
||||
/* send status (values already copied in the section above) */
|
||||
mavlink_msg_sys_status_send(chan,
|
||||
v_status.onboard_control_sensors_present,
|
||||
v_status.onboard_control_sensors_enabled,
|
||||
v_status.onboard_control_sensors_health,
|
||||
v_status.load,
|
||||
v_status.voltage_battery * 1000.0f,
|
||||
v_status.current_battery * 1000.0f,
|
||||
v_status.battery_remaining,
|
||||
v_status.drop_rate_comm,
|
||||
v_status.errors_comm,
|
||||
v_status.errors_count1,
|
||||
v_status.errors_count2,
|
||||
v_status.errors_count3,
|
||||
v_status.errors_count4);
|
||||
lowspeed_counter = 0;
|
||||
}
|
||||
|
||||
lowspeed_counter++;
|
||||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
/* check if waypoint has been reached against the last positions */
|
||||
mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
|
||||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
/* send parameters at 20 Hz (if queued for sending) */
|
||||
mavlink_pm_queued_send();
|
||||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
if (baudrate > 57600) {
|
||||
mavlink_pm_queued_send();
|
||||
}
|
||||
|
||||
/* sleep 10 ms */
|
||||
usleep(10000);
|
||||
|
||||
/* send one string at 10 Hz */
|
||||
if (!mavlink_logbuffer_is_empty(&lb)) {
|
||||
struct mavlink_logmessage msg;
|
||||
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
|
||||
|
||||
if (lb_ret == OK) {
|
||||
mavlink_missionlib_send_gcs_string(msg.text);
|
||||
}
|
||||
}
|
||||
|
||||
/* sleep 15 ms */
|
||||
usleep(15000);
|
||||
}
|
||||
|
||||
/* wait for threads to complete */
|
||||
pthread_join(receive_thread, NULL);
|
||||
pthread_join(uorb_receive_thread, NULL);
|
||||
|
||||
/* Reset the UART flags to original state */
|
||||
if (!usb_uart)
|
||||
tcsetattr(uart, TCSANOW, &uart_config_original);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
usage()
|
||||
{
|
||||
fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
|
||||
" mavlink stop\n"
|
||||
" mavlink status\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int mavlink_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 2) {
|
||||
warnx("missing command");
|
||||
usage();
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (thread_running)
|
||||
errx(0, "mavlink already running\n");
|
||||
|
||||
thread_should_exit = false;
|
||||
mavlink_task = task_spawn("mavlink",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
mavlink_thread_main,
|
||||
(const char **)argv);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
|
||||
while (thread_running) {
|
||||
usleep(200000);
|
||||
printf(".");
|
||||
}
|
||||
|
||||
warnx("terminated.");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
errx(0, "running");
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
|
||||
warnx("unrecognized command");
|
||||
usage();
|
||||
/* not getting here */
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,81 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_bridge_header
|
||||
* MAVLink bridge header for UART access.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/* MAVLink adapter header */
|
||||
#ifndef MAVLINK_BRIDGE_HEADER_H
|
||||
#define MAVLINK_BRIDGE_HEADER_H
|
||||
|
||||
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
/* use efficient approach, see mavlink_helpers.h */
|
||||
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
|
||||
|
||||
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
|
||||
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
|
||||
|
||||
#include <v1.0/mavlink_types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
/* Struct that stores the communication settings of this system.
|
||||
you can also define / alter these settings elsewhere, as long
|
||||
as they're included BEFORE mavlink.h.
|
||||
So you can set the
|
||||
|
||||
mavlink_system.sysid = 100; // System ID, 1-255
|
||||
mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
|
||||
|
||||
Lines also in your main.c, e.g. by reading these parameter from EEPROM.
|
||||
*/
|
||||
extern mavlink_system_t mavlink_system;
|
||||
|
||||
/**
|
||||
* @brief Send multiple chars (uint8_t) over a comm channel
|
||||
*
|
||||
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
|
||||
* @param ch Character to send
|
||||
*/
|
||||
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
|
||||
|
||||
mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
|
||||
mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
|
||||
|
||||
#endif /* MAVLINK_BRIDGE_HEADER_H */
|
||||
@@ -0,0 +1,61 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_hil.h
|
||||
* Hardware-in-the-loop simulation support.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
extern bool mavlink_hil_enabled;
|
||||
|
||||
extern struct vehicle_global_position_s hil_global_pos;
|
||||
extern struct vehicle_attitude_s hil_attitude;
|
||||
extern struct sensor_combined_s hil_sensors;
|
||||
extern struct vehicle_gps_position_s hil_gps;
|
||||
extern orb_advert_t pub_hil_global_pos;
|
||||
extern orb_advert_t pub_hil_attitude;
|
||||
extern orb_advert_t pub_hil_sensors;
|
||||
extern orb_advert_t pub_hil_gps;
|
||||
|
||||
/**
|
||||
* Enable / disable Hardware in the Loop simulation mode.
|
||||
*
|
||||
* @param hil_enabled The new HIL enable/disable state.
|
||||
* @return OK if the HIL state changed, ERROR if the
|
||||
* requested change could not be made or was
|
||||
* redundant.
|
||||
*/
|
||||
extern int set_hil_on_off(bool hil_enabled);
|
||||
@@ -0,0 +1,89 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_log.c
|
||||
* MAVLink text logging.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
|
||||
{
|
||||
lb->size = size;
|
||||
lb->start = 0;
|
||||
lb->count = 0;
|
||||
lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
|
||||
}
|
||||
|
||||
int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
|
||||
{
|
||||
return lb->count == (int)lb->size;
|
||||
}
|
||||
|
||||
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
|
||||
{
|
||||
return lb->count == 0;
|
||||
}
|
||||
|
||||
void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
|
||||
{
|
||||
int end = (lb->start + lb->count) % lb->size;
|
||||
memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
|
||||
|
||||
if (mavlink_logbuffer_is_full(lb)) {
|
||||
lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
|
||||
|
||||
} else {
|
||||
++lb->count;
|
||||
}
|
||||
}
|
||||
|
||||
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
|
||||
{
|
||||
if (!mavlink_logbuffer_is_empty(lb)) {
|
||||
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
|
||||
lb->start = (lb->start + 1) % lb->size;
|
||||
--lb->count;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,231 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_parameters.c
|
||||
* MAVLink parameter protocol implementation (BSD-relicensed).
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include "mavlink_parameters.h"
|
||||
#include <uORB/uORB.h>
|
||||
#include "math.h" /* isinf / isnan checks */
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
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)
|
||||
{
|
||||
mavlink_pm_send_param(param);
|
||||
usleep(*(unsigned int *)arg);
|
||||
}
|
||||
|
||||
void mavlink_pm_send_all_params(unsigned int delay)
|
||||
{
|
||||
unsigned int dbuf = delay;
|
||||
param_foreach(&mavlink_pm_callback, &dbuf, false);
|
||||
}
|
||||
|
||||
int mavlink_pm_queued_send()
|
||||
{
|
||||
if (mavlink_param_queue_index < param_count()) {
|
||||
mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
|
||||
mavlink_param_queue_index++;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
void mavlink_pm_start_queued_send()
|
||||
{
|
||||
mavlink_param_queue_index = 0;
|
||||
}
|
||||
|
||||
int 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)
|
||||
{
|
||||
return mavlink_pm_send_param(param_find(name));
|
||||
}
|
||||
|
||||
int mavlink_pm_send_param(param_t param)
|
||||
{
|
||||
if (param == PARAM_INVALID) return 1;
|
||||
|
||||
/* buffers for param transmission */
|
||||
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
|
||||
float val_buf;
|
||||
static mavlink_message_t tx_msg;
|
||||
|
||||
/* query parameter type */
|
||||
param_type_t type = param_type(param);
|
||||
/* copy parameter name */
|
||||
strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
|
||||
/*
|
||||
* Map onboard parameter type to MAVLink type,
|
||||
* endianess matches (both little endian)
|
||||
*/
|
||||
uint8_t mavlink_type;
|
||||
|
||||
if (type == PARAM_TYPE_INT32) {
|
||||
mavlink_type = MAVLINK_TYPE_INT32_T;
|
||||
|
||||
} else if (type == PARAM_TYPE_FLOAT) {
|
||||
mavlink_type = MAVLINK_TYPE_FLOAT;
|
||||
|
||||
} else {
|
||||
mavlink_type = MAVLINK_TYPE_FLOAT;
|
||||
}
|
||||
|
||||
/*
|
||||
* get param value, since MAVLink encodes float and int params in the same
|
||||
* space during transmission, copy param onto float val_buf
|
||||
*/
|
||||
|
||||
int ret;
|
||||
|
||||
if ((ret = param_get(param, &val_buf)) != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
|
||||
mavlink_system.compid,
|
||||
MAVLINK_COMM_0,
|
||||
&tx_msg,
|
||||
name_buf,
|
||||
val_buf,
|
||||
mavlink_type,
|
||||
param_count(),
|
||||
param_get_index(param));
|
||||
ret = mavlink_missionlib_send_message(&tx_msg);
|
||||
return ret;
|
||||
}
|
||||
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
|
||||
/* Start sending parameters */
|
||||
mavlink_pm_start_queued_send();
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
|
||||
} break;
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET: {
|
||||
|
||||
/* Handle parameter setting */
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
|
||||
mavlink_param_set_t mavlink_param_set;
|
||||
mavlink_msg_param_set_decode(msg, &mavlink_param_set);
|
||||
|
||||
if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
|
||||
/* local name buffer to enforce null-terminated string */
|
||||
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
|
||||
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
/* enforce null termination */
|
||||
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
|
||||
/* attempt to find parameter, set and send it */
|
||||
param_t param = param_find(name);
|
||||
|
||||
if (param == PARAM_INVALID) {
|
||||
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
|
||||
sprintf(buf, "[mavlink pm] unknown: %s", name);
|
||||
mavlink_missionlib_send_gcs_string(buf);
|
||||
|
||||
} else {
|
||||
/* set and send parameter */
|
||||
param_set(param, &(mavlink_param_set.param_value));
|
||||
mavlink_pm_send_param(param);
|
||||
}
|
||||
}
|
||||
}
|
||||
} break;
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
|
||||
mavlink_param_request_read_t mavlink_param_request_read;
|
||||
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
|
||||
|
||||
if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
|
||||
/* when no index is given, loop through string ids and compare them */
|
||||
if (mavlink_param_request_read.param_index == -1) {
|
||||
/* local name buffer to enforce null-terminated string */
|
||||
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
|
||||
strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
/* enforce null termination */
|
||||
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
|
||||
/* attempt to find parameter and send it */
|
||||
mavlink_pm_send_param_for_name(name);
|
||||
|
||||
} else {
|
||||
/* when index is >= 0, send this parameter again */
|
||||
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
|
||||
}
|
||||
}
|
||||
|
||||
} break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_parameters.h
|
||||
* MAVLink parameter protocol definitions (BSD-relicensed).
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/* This assumes you have the mavlink headers on your include path
|
||||
or in the same folder as this source file */
|
||||
|
||||
|
||||
#include <v1.0/mavlink_types.h>
|
||||
#include <stdbool.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Handle parameter related messages.
|
||||
*/
|
||||
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Send all parameters at once.
|
||||
*
|
||||
* This function blocks until all parameters have been sent.
|
||||
* it delays each parameter by the passed amount of microseconds.
|
||||
*
|
||||
* @param delay The delay in us between sending all parameters.
|
||||
*/
|
||||
void mavlink_pm_send_all_params(unsigned int delay);
|
||||
|
||||
/**
|
||||
* Send one parameter.
|
||||
*
|
||||
* @param param The parameter id to send.
|
||||
* @return zero on success, nonzero on failure.
|
||||
*/
|
||||
int mavlink_pm_send_param(param_t param);
|
||||
|
||||
/**
|
||||
* Send one parameter identified by index.
|
||||
*
|
||||
* @param index The index of the parameter to send.
|
||||
* @return zero on success, nonzero else.
|
||||
*/
|
||||
int mavlink_pm_send_param_for_index(uint16_t index);
|
||||
|
||||
/**
|
||||
* Send one parameter identified by name.
|
||||
*
|
||||
* @param name The index of the parameter to send.
|
||||
* @return zero on success, nonzero else.
|
||||
*/
|
||||
int mavlink_pm_send_param_for_name(const char *name);
|
||||
|
||||
/**
|
||||
* Send a queue of parameters, one parameter per function call.
|
||||
*
|
||||
* @return zero on success, nonzero on failure
|
||||
*/
|
||||
int mavlink_pm_queued_send(void);
|
||||
|
||||
/**
|
||||
* Start sending the parameter queue.
|
||||
*
|
||||
* This function will not directly send parameters, but instead
|
||||
* activate the sending of one parameter on each call of
|
||||
* mavlink_pm_queued_send().
|
||||
* @see mavlink_pm_queued_send()
|
||||
*/
|
||||
void mavlink_pm_start_queued_send(void);
|
||||
@@ -0,0 +1,718 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_receiver.c
|
||||
* MAVLink protocol message receive and dispatch
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "mavlink_parameters.h"
|
||||
#include "util.h"
|
||||
|
||||
/* XXX should be in a header somewhere */
|
||||
pthread_t receive_start(int uart);
|
||||
|
||||
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_attitude_s hil_attitude;
|
||||
struct vehicle_gps_position_s hil_gps;
|
||||
struct sensor_combined_s hil_sensors;
|
||||
orb_advert_t pub_hil_global_pos = -1;
|
||||
orb_advert_t pub_hil_attitude = -1;
|
||||
orb_advert_t pub_hil_gps = -1;
|
||||
orb_advert_t pub_hil_sensors = -1;
|
||||
|
||||
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;
|
||||
|
||||
extern bool gcs_link;
|
||||
|
||||
static void
|
||||
handle_message(mavlink_message_t *msg)
|
||||
{
|
||||
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||
|
||||
mavlink_command_long_t cmd_mavlink;
|
||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|
||||
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
|
||||
//check for MAVLINK terminate command
|
||||
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
|
||||
/* This is the link shutdown command, terminate mavlink */
|
||||
printf("[mavlink] Terminating .. \n");
|
||||
fflush(stdout);
|
||||
usleep(50000);
|
||||
|
||||
/* terminate other threads and this thread */
|
||||
thread_should_exit = true;
|
||||
|
||||
} else {
|
||||
|
||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = cmd_mavlink.param1;
|
||||
vcmd.param2 = cmd_mavlink.param2;
|
||||
vcmd.param3 = cmd_mavlink.param3;
|
||||
vcmd.param4 = cmd_mavlink.param4;
|
||||
vcmd.param5 = cmd_mavlink.param5;
|
||||
vcmd.param6 = cmd_mavlink.param6;
|
||||
vcmd.param7 = cmd_mavlink.param7;
|
||||
vcmd.command = cmd_mavlink.command;
|
||||
vcmd.target_system = cmd_mavlink.target_system;
|
||||
vcmd.target_component = cmd_mavlink.target_component;
|
||||
vcmd.source_system = msg->sysid;
|
||||
vcmd.source_component = msg->compid;
|
||||
vcmd.confirmation = cmd_mavlink.confirmation;
|
||||
|
||||
/* check if topic is advertised */
|
||||
if (cmd_pub <= 0) {
|
||||
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
}
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
|
||||
mavlink_optical_flow_t flow;
|
||||
mavlink_msg_optical_flow_decode(msg, &flow);
|
||||
|
||||
struct optical_flow_s f;
|
||||
|
||||
f.timestamp = flow.time_usec;
|
||||
f.flow_raw_x = flow.flow_x;
|
||||
f.flow_raw_y = flow.flow_y;
|
||||
f.flow_comp_x_m = flow.flow_comp_m_x;
|
||||
f.flow_comp_y_m = flow.flow_comp_m_y;
|
||||
f.ground_distance_m = flow.ground_distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
|
||||
/* check if topic is advertised */
|
||||
if (flow_pub <= 0) {
|
||||
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||
|
||||
} else {
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
|
||||
/* Set mode on request */
|
||||
mavlink_set_mode_t new_mode;
|
||||
mavlink_msg_set_mode_decode(msg, &new_mode);
|
||||
|
||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = new_mode.base_mode;
|
||||
vcmd.param2 = new_mode.custom_mode;
|
||||
vcmd.param3 = 0;
|
||||
vcmd.param4 = 0;
|
||||
vcmd.param5 = 0;
|
||||
vcmd.param6 = 0;
|
||||
vcmd.param7 = 0;
|
||||
vcmd.command = MAV_CMD_DO_SET_MODE;
|
||||
vcmd.target_system = new_mode.target_system;
|
||||
vcmd.target_component = MAV_COMP_ID_ALL;
|
||||
vcmd.source_system = msg->sysid;
|
||||
vcmd.source_component = msg->compid;
|
||||
vcmd.confirmation = 1;
|
||||
|
||||
/* check if topic is advertised */
|
||||
if (cmd_pub <= 0) {
|
||||
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
/* create command */
|
||||
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
|
||||
}
|
||||
}
|
||||
|
||||
/* Handle Vicon position estimates */
|
||||
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
|
||||
mavlink_vicon_position_estimate_t pos;
|
||||
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
|
||||
|
||||
vicon_position.timestamp = hrt_absolute_time();
|
||||
|
||||
vicon_position.x = pos.x;
|
||||
vicon_position.y = pos.y;
|
||||
vicon_position.z = pos.z;
|
||||
|
||||
vicon_position.roll = pos.roll;
|
||||
vicon_position.pitch = pos.pitch;
|
||||
vicon_position.yaw = pos.yaw;
|
||||
|
||||
if (vicon_position_pub <= 0) {
|
||||
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
|
||||
}
|
||||
}
|
||||
|
||||
/* Handle quadrotor motor setpoints */
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
|
||||
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
|
||||
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
|
||||
|
||||
if (mavlink_system.sysid < 4) {
|
||||
|
||||
/* switch to a receiving link mode */
|
||||
gcs_link = false;
|
||||
|
||||
/*
|
||||
* rate control mode - defined by MAVLink
|
||||
*/
|
||||
|
||||
uint8_t ml_mode = 0;
|
||||
bool ml_armed = false;
|
||||
|
||||
switch (quad_motors_setpoint.mode) {
|
||||
case 0:
|
||||
ml_armed = false;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
|
||||
ml_armed = true;
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
|
||||
ml_armed = true;
|
||||
|
||||
break;
|
||||
|
||||
case 3:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
|
||||
break;
|
||||
|
||||
case 4:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
|
||||
break;
|
||||
}
|
||||
|
||||
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
|
||||
|
||||
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
|
||||
ml_armed = false;
|
||||
}
|
||||
|
||||
offboard_control_sp.armed = ml_armed;
|
||||
offboard_control_sp.mode = ml_mode;
|
||||
|
||||
offboard_control_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* check if topic has to be advertised */
|
||||
if (offboard_control_sp_pub <= 0) {
|
||||
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
|
||||
} else {
|
||||
/* Publish */
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Only decode hil messages in HIL mode.
|
||||
*
|
||||
* The HIL mode is enabled by the HIL bit flag
|
||||
* in the system mode. Either send a set mode
|
||||
* COMMAND_LONG message or a SET_MODE message
|
||||
*/
|
||||
|
||||
if (mavlink_hil_enabled) {
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
/* TODO, set ground_press/ temp during calib */
|
||||
static const float ground_press = 1013.25f; // mbar
|
||||
static const float ground_tempC = 21.0f;
|
||||
static const float ground_alt = 0.0f;
|
||||
static const float T0 = 273.15f;
|
||||
static const float R = 287.05f;
|
||||
static const float g = 9.806f;
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
|
||||
|
||||
mavlink_raw_imu_t imu;
|
||||
mavlink_msg_raw_imu_decode(msg, &imu);
|
||||
|
||||
/* packet counter */
|
||||
static uint16_t hil_counter = 0;
|
||||
static uint16_t hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* sensors general */
|
||||
hil_sensors.timestamp = imu.time_usec;
|
||||
|
||||
/* hil gyro */
|
||||
static const float mrad2rad = 1.0e-3f;
|
||||
hil_sensors.gyro_counter = hil_counter;
|
||||
hil_sensors.gyro_raw[0] = imu.xgyro;
|
||||
hil_sensors.gyro_raw[1] = imu.ygyro;
|
||||
hil_sensors.gyro_raw[2] = imu.zgyro;
|
||||
hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
|
||||
hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
|
||||
hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
|
||||
|
||||
/* accelerometer */
|
||||
hil_sensors.accelerometer_counter = hil_counter;
|
||||
static const float mg2ms2 = 9.8f / 1000.0f;
|
||||
hil_sensors.accelerometer_raw[0] = imu.xacc;
|
||||
hil_sensors.accelerometer_raw[1] = imu.yacc;
|
||||
hil_sensors.accelerometer_raw[2] = imu.zacc;
|
||||
hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
|
||||
hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
|
||||
hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
|
||||
hil_sensors.accelerometer_mode = 0; // TODO what is this?
|
||||
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
|
||||
|
||||
/* adc */
|
||||
hil_sensors.adc_voltage_v[0] = 0;
|
||||
hil_sensors.adc_voltage_v[1] = 0;
|
||||
hil_sensors.adc_voltage_v[2] = 0;
|
||||
|
||||
/* magnetometer */
|
||||
float mga2ga = 1.0e-3f;
|
||||
hil_sensors.magnetometer_counter = hil_counter;
|
||||
hil_sensors.magnetometer_raw[0] = imu.xmag;
|
||||
hil_sensors.magnetometer_raw[1] = imu.ymag;
|
||||
hil_sensors.magnetometer_raw[2] = imu.zmag;
|
||||
hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
|
||||
hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
|
||||
hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
|
||||
hil_sensors.magnetometer_range_ga = 32.7f; // int16
|
||||
hil_sensors.magnetometer_mode = 0; // TODO what is this
|
||||
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
|
||||
|
||||
// increment counters
|
||||
hil_counter += 1 ;
|
||||
hil_frames += 1 ;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 10000000) {
|
||||
printf("receiving hil imu at %d hz\n", hil_frames/10);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
|
||||
|
||||
mavlink_highres_imu_t imu;
|
||||
mavlink_msg_highres_imu_decode(msg, &imu);
|
||||
|
||||
/* packet counter */
|
||||
static uint16_t hil_counter = 0;
|
||||
static uint16_t hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* sensors general */
|
||||
hil_sensors.timestamp = hrt_absolute_time();
|
||||
|
||||
/* hil gyro */
|
||||
static const float mrad2rad = 1.0e-3f;
|
||||
hil_sensors.gyro_counter = hil_counter;
|
||||
hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
|
||||
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
|
||||
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
|
||||
hil_sensors.gyro_rad_s[0] = imu.xgyro;
|
||||
hil_sensors.gyro_rad_s[1] = imu.ygyro;
|
||||
hil_sensors.gyro_rad_s[2] = imu.zgyro;
|
||||
|
||||
/* accelerometer */
|
||||
hil_sensors.accelerometer_counter = hil_counter;
|
||||
static const float mg2ms2 = 9.8f / 1000.0f;
|
||||
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
|
||||
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
|
||||
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
|
||||
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
|
||||
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
|
||||
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
|
||||
hil_sensors.accelerometer_mode = 0; // TODO what is this?
|
||||
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
|
||||
|
||||
/* adc */
|
||||
hil_sensors.adc_voltage_v[0] = 0;
|
||||
hil_sensors.adc_voltage_v[1] = 0;
|
||||
hil_sensors.adc_voltage_v[2] = 0;
|
||||
|
||||
/* magnetometer */
|
||||
float mga2ga = 1.0e-3f;
|
||||
hil_sensors.magnetometer_counter = hil_counter;
|
||||
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
|
||||
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
|
||||
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
|
||||
hil_sensors.magnetometer_ga[0] = imu.xmag;
|
||||
hil_sensors.magnetometer_ga[1] = imu.ymag;
|
||||
hil_sensors.magnetometer_ga[2] = imu.zmag;
|
||||
hil_sensors.magnetometer_range_ga = 32.7f; // int16
|
||||
hil_sensors.magnetometer_mode = 0; // TODO what is this
|
||||
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
|
||||
|
||||
hil_sensors.baro_pres_mbar = imu.abs_pressure;
|
||||
|
||||
float tempC = imu.temperature;
|
||||
float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
|
||||
float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure);
|
||||
|
||||
hil_sensors.baro_alt_meter = h;
|
||||
hil_sensors.baro_temp_celcius = imu.temperature;
|
||||
|
||||
hil_sensors.gyro_counter = hil_counter;
|
||||
hil_sensors.magnetometer_counter = hil_counter;
|
||||
hil_sensors.accelerometer_counter = hil_counter;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
|
||||
|
||||
// increment counters
|
||||
hil_counter++;
|
||||
hil_frames++;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 10000000) {
|
||||
printf("receiving hil imu at %d hz\n", hil_frames/10);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
|
||||
|
||||
mavlink_gps_raw_int_t gps;
|
||||
mavlink_msg_gps_raw_int_decode(msg, &gps);
|
||||
|
||||
/* packet counter */
|
||||
static uint16_t hil_counter = 0;
|
||||
static uint16_t hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* gps */
|
||||
hil_gps.timestamp_position = gps.time_usec;
|
||||
hil_gps.time_gps_usec = gps.time_usec;
|
||||
hil_gps.lat = gps.lat;
|
||||
hil_gps.lon = gps.lon;
|
||||
hil_gps.alt = gps.alt;
|
||||
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
|
||||
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
|
||||
hil_gps.s_variance_m_s = 5.0f;
|
||||
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
|
||||
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
|
||||
|
||||
/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
|
||||
float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
|
||||
/* go back to -PI..PI */
|
||||
if (heading_rad > M_PI_F)
|
||||
heading_rad -= 2.0f * M_PI_F;
|
||||
hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
|
||||
hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
|
||||
hil_gps.vel_d_m_s = 0.0f;
|
||||
hil_gps.vel_ned_valid = true;
|
||||
/* COG (course over ground) is speced as -PI..+PI */
|
||||
hil_gps.cog_rad = heading_rad;
|
||||
hil_gps.fix_type = gps.fix_type;
|
||||
hil_gps.satellites_visible = gps.satellites_visible;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
|
||||
|
||||
// increment counters
|
||||
hil_counter += 1 ;
|
||||
hil_frames += 1 ;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 10000000) {
|
||||
printf("receiving hil gps at %d hz\n", hil_frames/10);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
|
||||
|
||||
mavlink_raw_pressure_t press;
|
||||
mavlink_msg_raw_pressure_decode(msg, &press);
|
||||
|
||||
/* packet counter */
|
||||
static uint16_t hil_counter = 0;
|
||||
static uint16_t hil_frames = 0;
|
||||
static uint64_t old_timestamp = 0;
|
||||
|
||||
/* sensors general */
|
||||
hil_sensors.timestamp = press.time_usec;
|
||||
|
||||
/* baro */
|
||||
|
||||
float tempC = press.temperature / 100.0f;
|
||||
float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
|
||||
float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
|
||||
hil_sensors.baro_counter = hil_counter;
|
||||
hil_sensors.baro_pres_mbar = press.press_abs;
|
||||
hil_sensors.baro_alt_meter = h;
|
||||
hil_sensors.baro_temp_celcius = tempC;
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
|
||||
|
||||
// increment counters
|
||||
hil_counter += 1 ;
|
||||
hil_frames += 1 ;
|
||||
|
||||
// output
|
||||
if ((timestamp - old_timestamp) > 10000000) {
|
||||
printf("receiving hil pressure at %d hz\n", hil_frames/10);
|
||||
old_timestamp = timestamp;
|
||||
hil_frames = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
|
||||
|
||||
mavlink_hil_state_t hil_state;
|
||||
mavlink_msg_hil_state_decode(msg, &hil_state);
|
||||
|
||||
/* Calculate Rotation Matrix */
|
||||
//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
|
||||
|
||||
if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
|
||||
//TODO: assuming low pitch and roll values for now
|
||||
hil_attitude.R[0][0] = cosf(hil_state.yaw);
|
||||
hil_attitude.R[0][1] = sinf(hil_state.yaw);
|
||||
hil_attitude.R[0][2] = 0.0f;
|
||||
|
||||
hil_attitude.R[1][0] = -sinf(hil_state.yaw);
|
||||
hil_attitude.R[1][1] = cosf(hil_state.yaw);
|
||||
hil_attitude.R[1][2] = 0.0f;
|
||||
|
||||
hil_attitude.R[2][0] = 0.0f;
|
||||
hil_attitude.R[2][1] = 0.0f;
|
||||
hil_attitude.R[2][2] = 1.0f;
|
||||
|
||||
hil_attitude.R_valid = true;
|
||||
}
|
||||
|
||||
hil_global_pos.lat = hil_state.lat;
|
||||
hil_global_pos.lon = hil_state.lon;
|
||||
hil_global_pos.alt = hil_state.alt / 1000.0f;
|
||||
hil_global_pos.vx = hil_state.vx / 100.0f;
|
||||
hil_global_pos.vy = hil_state.vy / 100.0f;
|
||||
hil_global_pos.vz = hil_state.vz / 100.0f;
|
||||
|
||||
|
||||
/* set timestamp and notify processes (broadcast) */
|
||||
hil_global_pos.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
|
||||
|
||||
hil_attitude.roll = hil_state.roll;
|
||||
hil_attitude.pitch = hil_state.pitch;
|
||||
hil_attitude.yaw = hil_state.yaw;
|
||||
hil_attitude.rollspeed = hil_state.rollspeed;
|
||||
hil_attitude.pitchspeed = hil_state.pitchspeed;
|
||||
hil_attitude.yawspeed = hil_state.yawspeed;
|
||||
|
||||
/* set timestamp and notify processes (broadcast) */
|
||||
hil_attitude.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
|
||||
mavlink_manual_control_t man;
|
||||
mavlink_msg_manual_control_decode(msg, &man);
|
||||
|
||||
struct rc_channels_s rc_hil;
|
||||
memset(&rc_hil, 0, sizeof(rc_hil));
|
||||
static orb_advert_t rc_pub = 0;
|
||||
|
||||
rc_hil.timestamp = hrt_absolute_time();
|
||||
rc_hil.chan_count = 4;
|
||||
|
||||
rc_hil.chan[0].scaled = man.x / 1000.0f;
|
||||
rc_hil.chan[1].scaled = man.y / 1000.0f;
|
||||
rc_hil.chan[2].scaled = man.r / 1000.0f;
|
||||
rc_hil.chan[3].scaled = man.z / 1000.0f;
|
||||
|
||||
struct manual_control_setpoint_s mc;
|
||||
static orb_advert_t mc_pub = 0;
|
||||
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
|
||||
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
|
||||
|
||||
mc.timestamp = rc_hil.timestamp;
|
||||
mc.roll = man.x / 1000.0f;
|
||||
mc.pitch = man.y / 1000.0f;
|
||||
mc.yaw = man.r / 1000.0f;
|
||||
mc.throttle = man.z / 1000.0f;
|
||||
|
||||
/* fake RC channels with manual control input from simulator */
|
||||
|
||||
|
||||
if (rc_pub == 0) {
|
||||
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
|
||||
}
|
||||
|
||||
if (mc_pub == 0) {
|
||||
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Receive data from UART.
|
||||
*/
|
||||
static void *
|
||||
receive_thread(void *arg)
|
||||
{
|
||||
int uart_fd = *((int *)arg);
|
||||
|
||||
const int timeout = 1000;
|
||||
uint8_t buf[32];
|
||||
|
||||
mavlink_message_t msg;
|
||||
|
||||
prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
|
||||
|
||||
if (poll(fds, 1, timeout) > 0) {
|
||||
/* non-blocking read. read may return negative values */
|
||||
ssize_t nread = read(uart_fd, buf, sizeof(buf));
|
||||
|
||||
/* if read failed, this loop won't execute */
|
||||
for (ssize_t i = 0; i < nread; i++) {
|
||||
if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
|
||||
/* handle generic messages and commands */
|
||||
handle_message(&msg);
|
||||
|
||||
/* Handle packet with waypoint component */
|
||||
mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
|
||||
|
||||
/* Handle packet with parameter component */
|
||||
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pthread_t
|
||||
receive_start(int uart)
|
||||
{
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
// set to non-blocking read
|
||||
int flags = fcntl(uart, F_GETFL, 0);
|
||||
fcntl(uart, F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
struct sched_param param;
|
||||
param.sched_priority = SCHED_PRIORITY_MAX - 40;
|
||||
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
|
||||
return thread;
|
||||
}
|
||||
@@ -0,0 +1,200 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file missionlib.h
|
||||
* MAVLink missionlib components
|
||||
*/
|
||||
|
||||
// XXX trim includes
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "util.h"
|
||||
#include "waypoints.h"
|
||||
#include "mavlink_parameters.h"
|
||||
|
||||
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
|
||||
|
||||
int
|
||||
mavlink_missionlib_send_message(mavlink_message_t *msg)
|
||||
{
|
||||
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
|
||||
|
||||
mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
mavlink_missionlib_send_gcs_string(const char *string)
|
||||
{
|
||||
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
|
||||
mavlink_statustext_t statustext;
|
||||
int i = 0;
|
||||
|
||||
while (i < len - 1) {
|
||||
statustext.text[i] = string[i];
|
||||
|
||||
if (string[i] == '\0')
|
||||
break;
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
if (i > 1) {
|
||||
/* Enforce null termination */
|
||||
statustext.text[i] = '\0';
|
||||
mavlink_message_t msg;
|
||||
|
||||
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
|
||||
return mavlink_missionlib_send_message(&msg);
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get system time since boot in microseconds
|
||||
*
|
||||
* @return the system time since boot in microseconds
|
||||
*/
|
||||
uint64_t mavlink_missionlib_get_system_timestamp()
|
||||
{
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
/**
|
||||
* This callback is executed each time a waypoint changes.
|
||||
*
|
||||
* It publishes the vehicle_global_position_setpoint_s or the
|
||||
* vehicle_local_position_setpoint_s topic, depending on the type of waypoint
|
||||
*/
|
||||
void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
||||
float param2, float param3, float param4, float param5_lat_x,
|
||||
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
|
||||
{
|
||||
static orb_advert_t global_position_setpoint_pub = -1;
|
||||
static orb_advert_t local_position_setpoint_pub = -1;
|
||||
char buf[50] = {0};
|
||||
|
||||
/* Update controller setpoints */
|
||||
if (frame == (int)MAV_FRAME_GLOBAL) {
|
||||
/* global, absolute waypoint */
|
||||
struct vehicle_global_position_setpoint_s sp;
|
||||
sp.lat = param5_lat_x * 1e7f;
|
||||
sp.lon = param6_lon_y * 1e7f;
|
||||
sp.altitude = param7_alt_z;
|
||||
sp.altitude_is_relative = false;
|
||||
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (global_position_setpoint_pub < 0) {
|
||||
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
|
||||
|
||||
} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
|
||||
/* global, relative alt (in relation to HOME) waypoint */
|
||||
struct vehicle_global_position_setpoint_s sp;
|
||||
sp.lat = param5_lat_x * 1e7f;
|
||||
sp.lon = param6_lon_y * 1e7f;
|
||||
sp.altitude = param7_alt_z;
|
||||
sp.altitude_is_relative = true;
|
||||
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (global_position_setpoint_pub < 0) {
|
||||
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
|
||||
|
||||
} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
|
||||
/* local, absolute waypoint */
|
||||
struct vehicle_local_position_setpoint_s sp;
|
||||
sp.x = param5_lat_x;
|
||||
sp.y = param6_lon_y;
|
||||
sp.z = param7_alt_z;
|
||||
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (local_position_setpoint_pub < 0) {
|
||||
local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
|
||||
}
|
||||
|
||||
sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
|
||||
}
|
||||
|
||||
mavlink_missionlib_send_gcs_string(buf);
|
||||
printf("%s\n", buf);
|
||||
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file missionlib.h
|
||||
* MAVLink mission helper library
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <v1.0/common/mavlink.h>
|
||||
|
||||
//extern void mavlink_wpm_send_message(mavlink_message_t *msg);
|
||||
//extern void mavlink_wpm_send_gcs_string(const char *string);
|
||||
//extern uint64_t mavlink_wpm_get_system_timestamp(void);
|
||||
extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
|
||||
extern int mavlink_missionlib_send_gcs_string(const char *string);
|
||||
extern uint64_t mavlink_missionlib_get_system_timestamp(void);
|
||||
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
||||
float param2, float param3, float param4, float param5_lat_x,
|
||||
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
|
||||
@@ -0,0 +1,47 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# MAVLink protocol to uORB interface process
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink
|
||||
SRCS += mavlink.c \
|
||||
missionlib.c \
|
||||
mavlink_parameters.c \
|
||||
mavlink_log.c \
|
||||
mavlink_receiver.c \
|
||||
orb_listener.c \
|
||||
waypoints.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
@@ -0,0 +1,778 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file orb_listener.c
|
||||
* Monitors ORB topics and sends update messages as appropriate.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
// XXX trim includes
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "waypoints.h"
|
||||
#include "orb_topics.h"
|
||||
#include "missionlib.h"
|
||||
#include "mavlink_hil.h"
|
||||
#include "util.h"
|
||||
|
||||
extern bool gcs_link;
|
||||
|
||||
struct vehicle_global_position_s global_pos;
|
||||
struct vehicle_local_position_s local_pos;
|
||||
struct vehicle_status_s v_status;
|
||||
struct rc_channels_s rc;
|
||||
struct rc_input_values rc_raw;
|
||||
struct actuator_armed_s armed;
|
||||
|
||||
struct mavlink_subscriptions mavlink_subs;
|
||||
|
||||
static int status_sub;
|
||||
static int rc_sub;
|
||||
|
||||
static unsigned int sensors_raw_counter;
|
||||
static unsigned int attitude_counter;
|
||||
static unsigned int gps_counter;
|
||||
|
||||
/*
|
||||
* Last sensor loop time
|
||||
* some outputs are better timestamped
|
||||
* with this "global" reference.
|
||||
*/
|
||||
static uint64_t last_sensor_timestamp;
|
||||
|
||||
static void *uorb_receive_thread(void *arg);
|
||||
|
||||
struct listener {
|
||||
void (*callback)(const struct listener *l);
|
||||
int *subp;
|
||||
uintptr_t arg;
|
||||
};
|
||||
|
||||
static void l_sensor_combined(const struct listener *l);
|
||||
static void l_vehicle_attitude(const struct listener *l);
|
||||
static void l_vehicle_gps_position(const struct listener *l);
|
||||
static void l_vehicle_status(const struct listener *l);
|
||||
static void l_rc_channels(const struct listener *l);
|
||||
static void l_input_rc(const struct listener *l);
|
||||
static void l_global_position(const struct listener *l);
|
||||
static void l_local_position(const struct listener *l);
|
||||
static void l_global_position_setpoint(const struct listener *l);
|
||||
static void l_local_position_setpoint(const struct listener *l);
|
||||
static void l_attitude_setpoint(const struct listener *l);
|
||||
static void l_actuator_outputs(const struct listener *l);
|
||||
static void l_actuator_armed(const struct listener *l);
|
||||
static void l_manual_control_setpoint(const struct listener *l);
|
||||
static void l_vehicle_attitude_controls(const struct listener *l);
|
||||
static void l_debug_key_value(const struct listener *l);
|
||||
static void l_optical_flow(const struct listener *l);
|
||||
static void l_vehicle_rates_setpoint(const struct listener *l);
|
||||
static void l_home(const struct listener *l);
|
||||
|
||||
static const struct listener listeners[] = {
|
||||
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
|
||||
{l_vehicle_attitude, &mavlink_subs.att_sub, 0},
|
||||
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
|
||||
{l_vehicle_status, &status_sub, 0},
|
||||
{l_rc_channels, &rc_sub, 0},
|
||||
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
|
||||
{l_global_position, &mavlink_subs.global_pos_sub, 0},
|
||||
{l_local_position, &mavlink_subs.local_pos_sub, 0},
|
||||
{l_global_position_setpoint, &mavlink_subs.spg_sub, 0},
|
||||
{l_local_position_setpoint, &mavlink_subs.spl_sub, 0},
|
||||
{l_attitude_setpoint, &mavlink_subs.spa_sub, 0},
|
||||
{l_actuator_outputs, &mavlink_subs.act_0_sub, 0},
|
||||
{l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
|
||||
{l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
|
||||
{l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
|
||||
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
|
||||
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
|
||||
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
|
||||
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
|
||||
{l_optical_flow, &mavlink_subs.optical_flow, 0},
|
||||
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
|
||||
{l_home, &mavlink_subs.home_sub, 0},
|
||||
};
|
||||
|
||||
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
|
||||
|
||||
void
|
||||
l_sensor_combined(const struct listener *l)
|
||||
{
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
/* copy sensors raw data into local buffer */
|
||||
orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);
|
||||
|
||||
last_sensor_timestamp = raw.timestamp;
|
||||
|
||||
/* mark individual fields as changed */
|
||||
uint16_t fields_updated = 0;
|
||||
static unsigned accel_counter = 0;
|
||||
static unsigned gyro_counter = 0;
|
||||
static unsigned mag_counter = 0;
|
||||
static unsigned baro_counter = 0;
|
||||
|
||||
if (accel_counter != raw.accelerometer_counter) {
|
||||
/* mark first three dimensions as changed */
|
||||
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
|
||||
accel_counter = raw.accelerometer_counter;
|
||||
}
|
||||
|
||||
if (gyro_counter != raw.gyro_counter) {
|
||||
/* mark second group dimensions as changed */
|
||||
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
|
||||
gyro_counter = raw.gyro_counter;
|
||||
}
|
||||
|
||||
if (mag_counter != raw.magnetometer_counter) {
|
||||
/* mark third group dimensions as changed */
|
||||
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
|
||||
mag_counter = raw.magnetometer_counter;
|
||||
}
|
||||
|
||||
if (baro_counter != raw.baro_counter) {
|
||||
/* mark last group dimensions as changed */
|
||||
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
|
||||
baro_counter = raw.baro_counter;
|
||||
}
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
|
||||
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
|
||||
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
|
||||
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
|
||||
raw.magnetometer_ga[0],
|
||||
raw.magnetometer_ga[1], raw.magnetometer_ga[2],
|
||||
raw.baro_pres_mbar, 0 /* no diff pressure yet */,
|
||||
raw.baro_alt_meter, raw.baro_temp_celcius,
|
||||
fields_updated);
|
||||
|
||||
sensors_raw_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
l_vehicle_attitude(const struct listener *l)
|
||||
{
|
||||
struct vehicle_attitude_s att;
|
||||
|
||||
|
||||
/* copy attitude data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
|
||||
|
||||
if (gcs_link)
|
||||
/* send sensor values */
|
||||
mavlink_msg_attitude_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
att.roll,
|
||||
att.pitch,
|
||||
att.yaw,
|
||||
att.rollspeed,
|
||||
att.pitchspeed,
|
||||
att.yawspeed);
|
||||
|
||||
attitude_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
l_vehicle_gps_position(const struct listener *l)
|
||||
{
|
||||
struct vehicle_gps_position_s gps;
|
||||
|
||||
/* copy gps data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
|
||||
|
||||
/* GPS COG is 0..2PI in degrees * 1e2 */
|
||||
float cog_deg = gps.cog_rad;
|
||||
if (cog_deg > M_PI_F)
|
||||
cog_deg -= 2.0f * M_PI_F;
|
||||
cog_deg *= M_RAD_TO_DEG_F;
|
||||
|
||||
|
||||
/* GPS position */
|
||||
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
|
||||
gps.timestamp_position,
|
||||
gps.fix_type,
|
||||
gps.lat,
|
||||
gps.lon,
|
||||
gps.alt,
|
||||
gps.eph_m * 1e2f, // from m to cm
|
||||
gps.epv_m * 1e2f, // from m to cm
|
||||
gps.vel_m_s * 1e2f, // from m/s to cm/s
|
||||
cog_deg * 1e2f, // from rad to deg * 100
|
||||
gps.satellites_visible);
|
||||
|
||||
/* update SAT info every 10 seconds */
|
||||
if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
|
||||
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
|
||||
gps.satellites_visible,
|
||||
gps.satellite_prn,
|
||||
gps.satellite_used,
|
||||
gps.satellite_elevation,
|
||||
gps.satellite_azimuth,
|
||||
gps.satellite_snr);
|
||||
}
|
||||
|
||||
gps_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
l_vehicle_status(const struct listener *l)
|
||||
{
|
||||
/* immediately communicate state changes back to user */
|
||||
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
|
||||
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
|
||||
|
||||
/* enable or disable HIL */
|
||||
set_hil_on_off(v_status.flag_hil_enabled);
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_mode = 0;
|
||||
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan,
|
||||
mavlink_system.type,
|
||||
MAV_AUTOPILOT_PX4,
|
||||
mavlink_mode,
|
||||
v_status.state_machine,
|
||||
mavlink_state);
|
||||
}
|
||||
|
||||
void
|
||||
l_rc_channels(const struct listener *l)
|
||||
{
|
||||
/* copy rc channels into local buffer */
|
||||
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
|
||||
// XXX Add RC channels scaled message here
|
||||
}
|
||||
|
||||
void
|
||||
l_input_rc(const struct listener *l)
|
||||
{
|
||||
/* copy rc channels into local buffer */
|
||||
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
|
||||
|
||||
if (gcs_link)
|
||||
/* Channels are sent in MAVLink main loop at a fixed interval */
|
||||
mavlink_msg_rc_channels_raw_send(chan,
|
||||
rc_raw.timestamp / 1000,
|
||||
0,
|
||||
(rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
|
||||
255);
|
||||
}
|
||||
|
||||
void
|
||||
l_global_position(const struct listener *l)
|
||||
{
|
||||
/* copy global position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
|
||||
|
||||
uint64_t timestamp = global_pos.timestamp;
|
||||
int32_t lat = global_pos.lat;
|
||||
int32_t lon = global_pos.lon;
|
||||
int32_t alt = (int32_t)(global_pos.alt * 1000);
|
||||
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
|
||||
int16_t vx = (int16_t)(global_pos.vx * 100.0f);
|
||||
int16_t vy = (int16_t)(global_pos.vy * 100.0f);
|
||||
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
|
||||
|
||||
/* heading in degrees * 10, from 0 to 36.000) */
|
||||
uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
|
||||
|
||||
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
|
||||
timestamp / 1000,
|
||||
lat,
|
||||
lon,
|
||||
alt,
|
||||
relative_alt,
|
||||
vx,
|
||||
vy,
|
||||
vz,
|
||||
hdg);
|
||||
}
|
||||
|
||||
void
|
||||
l_local_position(const struct listener *l)
|
||||
{
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
|
||||
local_pos.timestamp / 1000,
|
||||
local_pos.x,
|
||||
local_pos.y,
|
||||
local_pos.z,
|
||||
local_pos.vx,
|
||||
local_pos.vy,
|
||||
local_pos.vz);
|
||||
}
|
||||
|
||||
void
|
||||
l_global_position_setpoint(const struct listener *l)
|
||||
{
|
||||
struct vehicle_global_position_setpoint_s global_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
|
||||
|
||||
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
|
||||
|
||||
if (global_sp.altitude_is_relative)
|
||||
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
|
||||
coordinate_frame,
|
||||
global_sp.lat,
|
||||
global_sp.lon,
|
||||
global_sp.altitude,
|
||||
global_sp.yaw);
|
||||
}
|
||||
|
||||
void
|
||||
l_local_position_setpoint(const struct listener *l)
|
||||
{
|
||||
struct vehicle_local_position_setpoint_s local_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
|
||||
MAV_FRAME_LOCAL_NED,
|
||||
local_sp.x,
|
||||
local_sp.y,
|
||||
local_sp.z,
|
||||
local_sp.yaw);
|
||||
}
|
||||
|
||||
void
|
||||
l_attitude_setpoint(const struct listener *l)
|
||||
{
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
|
||||
att_sp.timestamp / 1000,
|
||||
att_sp.roll_body,
|
||||
att_sp.pitch_body,
|
||||
att_sp.yaw_body,
|
||||
att_sp.thrust);
|
||||
}
|
||||
|
||||
void
|
||||
l_vehicle_rates_setpoint(const struct listener *l)
|
||||
{
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
|
||||
/* copy local position data into local buffer */
|
||||
orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
|
||||
rates_sp.timestamp / 1000,
|
||||
rates_sp.roll,
|
||||
rates_sp.pitch,
|
||||
rates_sp.yaw,
|
||||
rates_sp.thrust);
|
||||
}
|
||||
|
||||
void
|
||||
l_actuator_outputs(const struct listener *l)
|
||||
{
|
||||
struct actuator_outputs_s act_outputs;
|
||||
|
||||
orb_id_t ids[] = {
|
||||
ORB_ID(actuator_outputs_0),
|
||||
ORB_ID(actuator_outputs_1),
|
||||
ORB_ID(actuator_outputs_2),
|
||||
ORB_ID(actuator_outputs_3)
|
||||
};
|
||||
|
||||
/* copy actuator data into local buffer */
|
||||
orb_copy(ids[l->arg], *l->subp, &act_outputs);
|
||||
|
||||
if (gcs_link) {
|
||||
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
|
||||
l->arg /* port number */,
|
||||
act_outputs.output[0],
|
||||
act_outputs.output[1],
|
||||
act_outputs.output[2],
|
||||
act_outputs.output[3],
|
||||
act_outputs.output[4],
|
||||
act_outputs.output[5],
|
||||
act_outputs.output[6],
|
||||
act_outputs.output[7]);
|
||||
|
||||
/* only send in HIL mode */
|
||||
if (mavlink_hil_enabled && armed.armed) {
|
||||
|
||||
/* translate the current syste state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_mode = 0;
|
||||
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
|
||||
|
||||
/* HIL message as per MAVLink spec */
|
||||
|
||||
/* scale / assign outputs depending on system type */
|
||||
|
||||
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
|
||||
-1,
|
||||
-1,
|
||||
-1,
|
||||
-1,
|
||||
mavlink_mode,
|
||||
0);
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
|
||||
-1,
|
||||
-1,
|
||||
mavlink_mode,
|
||||
0);
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
|
||||
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
|
||||
mavlink_mode,
|
||||
0);
|
||||
|
||||
} else {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
(act_outputs.output[0] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[1] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[2] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[3] - 1000.0f) / 1000.0f,
|
||||
(act_outputs.output[4] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[5] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[6] - 1500.0f) / 500.0f,
|
||||
(act_outputs.output[7] - 1500.0f) / 500.0f,
|
||||
mavlink_mode,
|
||||
0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
l_actuator_armed(const struct listener *l)
|
||||
{
|
||||
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
|
||||
}
|
||||
|
||||
void
|
||||
l_manual_control_setpoint(const struct listener *l)
|
||||
{
|
||||
struct manual_control_setpoint_s man_control;
|
||||
|
||||
/* copy manual control data into local buffer */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
|
||||
|
||||
if (gcs_link)
|
||||
mavlink_msg_manual_control_send(MAVLINK_COMM_0,
|
||||
mavlink_system.sysid,
|
||||
man_control.roll * 1000,
|
||||
man_control.pitch * 1000,
|
||||
man_control.yaw * 1000,
|
||||
man_control.throttle * 1000,
|
||||
0);
|
||||
}
|
||||
|
||||
void
|
||||
l_vehicle_attitude_controls(const struct listener *l)
|
||||
{
|
||||
struct actuator_controls_effective_s actuators;
|
||||
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
|
||||
|
||||
if (gcs_link) {
|
||||
/* send, add spaces so that string buffer is at least 10 chars long */
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
"eff ctrl0 ",
|
||||
actuators.control_effective[0]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
"eff ctrl1 ",
|
||||
actuators.control_effective[1]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
"eff ctrl2 ",
|
||||
actuators.control_effective[2]);
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
"eff ctrl3 ",
|
||||
actuators.control_effective[3]);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
l_debug_key_value(const struct listener *l)
|
||||
{
|
||||
struct debug_key_value_s debug;
|
||||
|
||||
orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug);
|
||||
|
||||
/* Enforce null termination */
|
||||
debug.key[sizeof(debug.key) - 1] = '\0';
|
||||
|
||||
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
|
||||
last_sensor_timestamp / 1000,
|
||||
debug.key,
|
||||
debug.value);
|
||||
}
|
||||
|
||||
void
|
||||
l_optical_flow(const struct listener *l)
|
||||
{
|
||||
struct optical_flow_s flow;
|
||||
|
||||
orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
|
||||
|
||||
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
|
||||
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
|
||||
}
|
||||
|
||||
void
|
||||
l_home(const struct listener *l)
|
||||
{
|
||||
struct home_position_s home;
|
||||
|
||||
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
|
||||
|
||||
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
|
||||
}
|
||||
|
||||
static void *
|
||||
uorb_receive_thread(void *arg)
|
||||
{
|
||||
/* Set thread name */
|
||||
prctl(PR_SET_NAME, "mavlink orb rcv", getpid());
|
||||
|
||||
/*
|
||||
* set up poll to block for new data,
|
||||
* wait for a maximum of 1000 ms (1 second)
|
||||
*/
|
||||
const int timeout = 1000;
|
||||
|
||||
/*
|
||||
* Initialise listener array.
|
||||
*
|
||||
* Might want to invoke each listener once to set initial state.
|
||||
*/
|
||||
struct pollfd fds[n_listeners];
|
||||
|
||||
for (unsigned i = 0; i < n_listeners; i++) {
|
||||
fds[i].fd = *listeners[i].subp;
|
||||
fds[i].events = POLLIN;
|
||||
|
||||
/* Invoke callback to set initial state */
|
||||
//listeners[i].callback(&listener[i]);
|
||||
}
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
int poll_ret = poll(fds, n_listeners, timeout);
|
||||
|
||||
/* handle the poll result */
|
||||
if (poll_ret == 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
|
||||
|
||||
} else if (poll_ret < 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
|
||||
|
||||
} else {
|
||||
|
||||
for (unsigned i = 0; i < n_listeners; i++) {
|
||||
if (fds[i].revents & POLLIN)
|
||||
listeners[i].callback(&listeners[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pthread_t
|
||||
uorb_receive_start(void)
|
||||
{
|
||||
/* --- SENSORS RAW VALUE --- */
|
||||
mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
/* rate limit set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- ATTITUDE VALUE --- */
|
||||
mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
/* rate limit set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- GPS VALUE --- */
|
||||
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */
|
||||
|
||||
/* --- HOME POSITION --- */
|
||||
mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
|
||||
orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */
|
||||
|
||||
/* --- SYSTEM STATE --- */
|
||||
status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
|
||||
|
||||
/* --- RC CHANNELS VALUE --- */
|
||||
rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
orb_set_interval(rc_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- RC RAW VALUE --- */
|
||||
mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
orb_set_interval(mavlink_subs.input_rc_sub, 100);
|
||||
|
||||
/* --- GLOBAL POS VALUE --- */
|
||||
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */
|
||||
|
||||
/* --- LOCAL POS VALUE --- */
|
||||
mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
|
||||
|
||||
/* --- GLOBAL SETPOINT VALUE --- */
|
||||
mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- LOCAL SETPOINT VALUE --- */
|
||||
mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
|
||||
orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- ATTITUDE SETPOINT VALUE --- */
|
||||
mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- RATES SETPOINT VALUE --- */
|
||||
mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
|
||||
orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */
|
||||
|
||||
/* --- ACTUATOR OUTPUTS --- */
|
||||
mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
|
||||
mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
|
||||
mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
|
||||
mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
|
||||
/* rate limits set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */
|
||||
orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- ACTUATOR ARMED VALUE --- */
|
||||
mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- MAPPED MANUAL CONTROL INPUTS --- */
|
||||
mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
/* rate limits set externally based on interface speed, set a basic default here */
|
||||
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- ACTUATOR CONTROL VALUE --- */
|
||||
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
|
||||
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- DEBUG VALUE OUTPUT --- */
|
||||
mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
|
||||
orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */
|
||||
|
||||
/* --- FLOW SENSOR --- */
|
||||
mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
|
||||
orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
|
||||
|
||||
/* start the listener loop */
|
||||
pthread_attr_t uorb_attr;
|
||||
pthread_attr_init(&uorb_attr);
|
||||
|
||||
/* Set stack size, needs less than 2k */
|
||||
pthread_attr_setstacksize(&uorb_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
|
||||
return thread;
|
||||
}
|
||||
@@ -0,0 +1,106 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file orb_topics.h
|
||||
* Common sets of topics subscribed to or published by the MAVLink driver,
|
||||
* and structures maintained by those subscriptions.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#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/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/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.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/manual_control_setpoint.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
struct mavlink_subscriptions {
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
int global_pos_sub;
|
||||
int act_0_sub;
|
||||
int act_1_sub;
|
||||
int act_2_sub;
|
||||
int act_3_sub;
|
||||
int gps_sub;
|
||||
int man_control_sp_sub;
|
||||
int armed_sub;
|
||||
int actuators_sub;
|
||||
int local_pos_sub;
|
||||
int spa_sub;
|
||||
int spl_sub;
|
||||
int spg_sub;
|
||||
int debug_key_value;
|
||||
int input_rc_sub;
|
||||
int optical_flow;
|
||||
int rates_setpoint_sub;
|
||||
int home_sub;
|
||||
};
|
||||
|
||||
extern struct mavlink_subscriptions mavlink_subs;
|
||||
|
||||
/** Global position */
|
||||
extern struct vehicle_global_position_s global_pos;
|
||||
|
||||
/** Local position */
|
||||
extern struct vehicle_local_position_s local_pos;
|
||||
|
||||
/** Vehicle status */
|
||||
extern struct vehicle_status_s v_status;
|
||||
|
||||
/** RC channels */
|
||||
extern struct rc_channels_s rc;
|
||||
|
||||
/** Actuator armed state */
|
||||
extern struct actuator_armed_s armed;
|
||||
|
||||
/** Worker thread starter */
|
||||
extern pthread_t uorb_receive_start(void);
|
||||
@@ -0,0 +1,54 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file util.h
|
||||
* Utility and helper functions and data.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/** MAVLink communications channel */
|
||||
extern uint8_t chan;
|
||||
|
||||
/** Shutdown marker */
|
||||
extern volatile bool thread_should_exit;
|
||||
|
||||
/** Waypoint storage */
|
||||
extern mavlink_wpm_storage *wpm;
|
||||
|
||||
/**
|
||||
* Translate the custom state into standard mavlink modes and state.
|
||||
*/
|
||||
extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,131 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file waypoints.h
|
||||
* MAVLink waypoint protocol definition (BSD-relicensed).
|
||||
*/
|
||||
|
||||
#ifndef WAYPOINTS_H_
|
||||
#define WAYPOINTS_H_
|
||||
|
||||
/* This assumes you have the mavlink headers on your include path
|
||||
or in the same folder as this source file */
|
||||
|
||||
#include <v1.0/mavlink_types.h>
|
||||
|
||||
#ifndef MAVLINK_SEND_UART_BYTES
|
||||
#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
|
||||
#endif
|
||||
extern mavlink_system_t mavlink_system;
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <stdbool.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.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;
|
||||
};
|
||||
|
||||
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
|
||||
|
||||
void mavlink_wpm_init(mavlink_wpm_storage *state);
|
||||
int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
|
||||
struct vehicle_local_position_s *local_pos);
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
|
||||
struct vehicle_local_position_s *local_pos);
|
||||
|
||||
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
||||
float param2, float param3, float param4, float param5_lat_x,
|
||||
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
|
||||
|
||||
#endif /* WAYPOINTS_H_ */
|
||||
@@ -0,0 +1,529 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink.c
|
||||
* MAVLink 1.0 protocol implementation.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "orb_topics.h"
|
||||
#include "util.h"
|
||||
|
||||
__EXPORT int mavlink_onboard_main(int argc, char *argv[]);
|
||||
|
||||
static int mavlink_thread_main(int argc, char *argv[]);
|
||||
|
||||
/* thread state */
|
||||
volatile bool thread_should_exit = false;
|
||||
static volatile bool thread_running = false;
|
||||
static int mavlink_task;
|
||||
|
||||
/* pthreads */
|
||||
static pthread_t receive_thread;
|
||||
|
||||
/* terminate MAVLink on user request - disabled by default */
|
||||
static bool mavlink_link_termination_allowed = false;
|
||||
|
||||
mavlink_system_t mavlink_system = {
|
||||
100,
|
||||
50,
|
||||
MAV_TYPE_QUADROTOR,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
}; // System ID, 1-255, Component/Subsystem ID, 1-255
|
||||
|
||||
/* XXX not widely used */
|
||||
uint8_t chan = MAVLINK_COMM_0;
|
||||
|
||||
/* XXX probably should be in a header... */
|
||||
extern pthread_t receive_start(int uart);
|
||||
|
||||
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 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);
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
|
||||
{
|
||||
/* process baud rate */
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
case 0: speed = B0; break;
|
||||
|
||||
case 50: speed = B50; break;
|
||||
|
||||
case 75: speed = B75; break;
|
||||
|
||||
case 110: speed = B110; break;
|
||||
|
||||
case 134: speed = B134; break;
|
||||
|
||||
case 150: speed = B150; break;
|
||||
|
||||
case 200: speed = B200; break;
|
||||
|
||||
case 300: speed = B300; break;
|
||||
|
||||
case 600: speed = B600; break;
|
||||
|
||||
case 1200: speed = B1200; break;
|
||||
|
||||
case 1800: speed = B1800; break;
|
||||
|
||||
case 2400: speed = B2400; break;
|
||||
|
||||
case 4800: speed = B4800; break;
|
||||
|
||||
case 9600: speed = B9600; break;
|
||||
|
||||
case 19200: speed = B19200; break;
|
||||
|
||||
case 38400: speed = B38400; break;
|
||||
|
||||
case 57600: speed = B57600; break;
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
case 230400: speed = B230400; break;
|
||||
|
||||
case 460800: speed = B460800; break;
|
||||
|
||||
case 921600: speed = B921600; break;
|
||||
|
||||
default:
|
||||
fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* open uart */
|
||||
printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
|
||||
uart = open(uart_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
*is_usb = false;
|
||||
|
||||
if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Fill the struct for the new configuration */
|
||||
tcgetattr(uart, &uart_config);
|
||||
|
||||
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* Set baud rate */
|
||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
|
||||
fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
|
||||
close(uart);
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else {
|
||||
*is_usb = true;
|
||||
}
|
||||
|
||||
return uart;
|
||||
}
|
||||
|
||||
void
|
||||
mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
|
||||
{
|
||||
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
|
||||
}
|
||||
|
||||
/*
|
||||
* Internal function to give access to the channel status for each channel
|
||||
*/
|
||||
mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
|
||||
{
|
||||
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
||||
return &m_mavlink_status[channel];
|
||||
}
|
||||
|
||||
/*
|
||||
* Internal function to give access to the channel buffer for each channel
|
||||
*/
|
||||
mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
|
||||
{
|
||||
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
|
||||
return &m_mavlink_buffer[channel];
|
||||
}
|
||||
|
||||
void mavlink_update_system(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
param_t param_system_id;
|
||||
param_t param_component_id;
|
||||
param_t param_system_type;
|
||||
|
||||
if (!initialized) {
|
||||
param_system_id = param_find("MAV_SYS_ID");
|
||||
param_component_id = param_find("MAV_COMP_ID");
|
||||
param_system_type = param_find("MAV_TYPE");
|
||||
}
|
||||
|
||||
/* update system and component id */
|
||||
int32_t system_id;
|
||||
param_get(param_system_id, &system_id);
|
||||
if (system_id > 0 && system_id < 255) {
|
||||
mavlink_system.sysid = system_id;
|
||||
}
|
||||
|
||||
int32_t component_id;
|
||||
param_get(param_component_id, &component_id);
|
||||
if (component_id > 0 && component_id < 255) {
|
||||
mavlink_system.compid = component_id;
|
||||
}
|
||||
|
||||
int32_t system_type;
|
||||
param_get(param_system_type, &system_type);
|
||||
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
|
||||
mavlink_system.type = system_type;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
|
||||
uint8_t *mavlink_state, uint8_t *mavlink_mode)
|
||||
{
|
||||
/* reset MAVLink mode bitfield */
|
||||
*mavlink_mode = 0;
|
||||
|
||||
/* set mode flags independent of system state */
|
||||
if (v_status->flag_control_manual_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
}
|
||||
|
||||
if (v_status->flag_hil_enabled) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||
}
|
||||
|
||||
/* set arming state */
|
||||
if (armed->armed) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
} else {
|
||||
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
}
|
||||
|
||||
switch (v_status->state_machine) {
|
||||
case SYSTEM_STATE_PREFLIGHT:
|
||||
if (v_status->flag_preflight_gyro_calibration ||
|
||||
v_status->flag_preflight_mag_calibration ||
|
||||
v_status->flag_preflight_accel_calibration) {
|
||||
*mavlink_state = MAV_STATE_CALIBRATING;
|
||||
} else {
|
||||
*mavlink_state = MAV_STATE_UNINIT;
|
||||
}
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STANDBY:
|
||||
*mavlink_state = MAV_STATE_STANDBY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_GROUND_READY:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_MANUAL:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STABILIZED:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_AUTO:
|
||||
*mavlink_state = MAV_STATE_ACTIVE;
|
||||
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_MISSION_ABORT:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_EMCY_LANDING:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_EMCY_CUTOFF:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_GROUND_ERROR:
|
||||
*mavlink_state = MAV_STATE_EMERGENCY;
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_REBOOT:
|
||||
*mavlink_state = MAV_STATE_POWEROFF;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* MAVLink Protocol main function.
|
||||
*/
|
||||
int mavlink_thread_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
char *device_name = "/dev/ttyS1";
|
||||
baudrate = 57600;
|
||||
|
||||
struct vehicle_status_s v_status;
|
||||
struct actuator_armed_s armed;
|
||||
|
||||
/* work around some stupidity in task_create's argv handling */
|
||||
argc -= 2;
|
||||
argv += 2;
|
||||
|
||||
while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'b':
|
||||
baudrate = strtoul(optarg, NULL, 10);
|
||||
if (baudrate == 0)
|
||||
errx(1, "invalid baud rate '%s'", optarg);
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
device_name = optarg;
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
mavlink_link_termination_allowed = true;
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
|
||||
break;
|
||||
|
||||
default:
|
||||
usage();
|
||||
}
|
||||
}
|
||||
|
||||
struct termios uart_config_original;
|
||||
bool usb_uart;
|
||||
|
||||
/* print welcome text */
|
||||
warnx("MAVLink v1.0 serial interface starting...");
|
||||
|
||||
/* inform about mode */
|
||||
warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
|
||||
|
||||
/* Flush stdout in case MAVLink is about to take it over */
|
||||
fflush(stdout);
|
||||
|
||||
/* default values for arguments */
|
||||
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
|
||||
if (uart < 0)
|
||||
err(1, "could not open %s", device_name);
|
||||
|
||||
/* Initialize system properties */
|
||||
mavlink_update_system();
|
||||
|
||||
/* start the MAVLink receiver */
|
||||
receive_thread = receive_start(uart);
|
||||
|
||||
thread_running = true;
|
||||
|
||||
/* arm counter to go off immediately */
|
||||
unsigned lowspeed_counter = 10;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* 1 Hz */
|
||||
if (lowspeed_counter == 10) {
|
||||
mavlink_update_system();
|
||||
|
||||
/* translate the current system state to mavlink state and mode */
|
||||
uint8_t mavlink_state = 0;
|
||||
uint8_t mavlink_mode = 0;
|
||||
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
|
||||
|
||||
/* send heartbeat */
|
||||
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
|
||||
|
||||
/* send status (values already copied in the section above) */
|
||||
mavlink_msg_sys_status_send(chan,
|
||||
v_status.onboard_control_sensors_present,
|
||||
v_status.onboard_control_sensors_enabled,
|
||||
v_status.onboard_control_sensors_health,
|
||||
v_status.load,
|
||||
v_status.voltage_battery * 1000.0f,
|
||||
v_status.current_battery * 1000.0f,
|
||||
v_status.battery_remaining,
|
||||
v_status.drop_rate_comm,
|
||||
v_status.errors_comm,
|
||||
v_status.errors_count1,
|
||||
v_status.errors_count2,
|
||||
v_status.errors_count3,
|
||||
v_status.errors_count4);
|
||||
lowspeed_counter = 0;
|
||||
}
|
||||
lowspeed_counter++;
|
||||
|
||||
/* sleep 1000 ms */
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
/* wait for threads to complete */
|
||||
pthread_join(receive_thread, NULL);
|
||||
|
||||
/* Reset the UART flags to original state */
|
||||
if (!usb_uart)
|
||||
tcsetattr(uart, TCSANOW, &uart_config_original);
|
||||
|
||||
thread_running = false;
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
usage()
|
||||
{
|
||||
fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
|
||||
" mavlink stop\n"
|
||||
" mavlink status\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int mavlink_onboard_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 2) {
|
||||
warnx("missing command");
|
||||
usage();
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (thread_running)
|
||||
errx(0, "mavlink already running\n");
|
||||
|
||||
thread_should_exit = false;
|
||||
mavlink_task = task_spawn("mavlink_onboard",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
mavlink_thread_main,
|
||||
(const char**)argv);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
while (thread_running) {
|
||||
usleep(200000);
|
||||
}
|
||||
warnx("terminated.");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
errx(0, "running");
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
|
||||
warnx("unrecognized command");
|
||||
usage();
|
||||
/* not getting here */
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,81 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_bridge_header
|
||||
* MAVLink bridge header for UART access.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/* MAVLink adapter header */
|
||||
#ifndef MAVLINK_BRIDGE_HEADER_H
|
||||
#define MAVLINK_BRIDGE_HEADER_H
|
||||
|
||||
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
/* use efficient approach, see mavlink_helpers.h */
|
||||
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
|
||||
|
||||
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
|
||||
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
|
||||
|
||||
#include <v1.0/mavlink_types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
/* Struct that stores the communication settings of this system.
|
||||
you can also define / alter these settings elsewhere, as long
|
||||
as they're included BEFORE mavlink.h.
|
||||
So you can set the
|
||||
|
||||
mavlink_system.sysid = 100; // System ID, 1-255
|
||||
mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
|
||||
|
||||
Lines also in your main.c, e.g. by reading these parameter from EEPROM.
|
||||
*/
|
||||
extern mavlink_system_t mavlink_system;
|
||||
|
||||
/**
|
||||
* @brief Send multiple chars (uint8_t) over a comm channel
|
||||
*
|
||||
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
|
||||
* @param ch Character to send
|
||||
*/
|
||||
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
|
||||
|
||||
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
|
||||
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
|
||||
|
||||
#endif /* MAVLINK_BRIDGE_HEADER_H */
|
||||
@@ -0,0 +1,331 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_receiver.c
|
||||
* MAVLink protocol message receive and dispatch
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
#include <string.h>
|
||||
#include "mavlink_bridge_header.h"
|
||||
#include <v1.0/common/mavlink.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
#include <nuttx/sched.h>
|
||||
#include <sys/prctl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "util.h"
|
||||
#include "orb_topics.h"
|
||||
|
||||
/* XXX should be in a header somewhere */
|
||||
pthread_t receive_start(int uart);
|
||||
|
||||
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_attitude_s hil_attitude;
|
||||
orb_advert_t pub_hil_global_pos = -1;
|
||||
orb_advert_t pub_hil_attitude = -1;
|
||||
|
||||
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;
|
||||
|
||||
extern bool gcs_link;
|
||||
|
||||
static void
|
||||
handle_message(mavlink_message_t *msg)
|
||||
{
|
||||
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||
|
||||
mavlink_command_long_t cmd_mavlink;
|
||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|
||||
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
|
||||
//check for MAVLINK terminate command
|
||||
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
|
||||
/* This is the link shutdown command, terminate mavlink */
|
||||
printf("[mavlink] Terminating .. \n");
|
||||
fflush(stdout);
|
||||
usleep(50000);
|
||||
|
||||
/* terminate other threads and this thread */
|
||||
thread_should_exit = true;
|
||||
|
||||
} else {
|
||||
|
||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = cmd_mavlink.param1;
|
||||
vcmd.param2 = cmd_mavlink.param2;
|
||||
vcmd.param3 = cmd_mavlink.param3;
|
||||
vcmd.param4 = cmd_mavlink.param4;
|
||||
vcmd.param5 = cmd_mavlink.param5;
|
||||
vcmd.param6 = cmd_mavlink.param6;
|
||||
vcmd.param7 = cmd_mavlink.param7;
|
||||
vcmd.command = cmd_mavlink.command;
|
||||
vcmd.target_system = cmd_mavlink.target_system;
|
||||
vcmd.target_component = cmd_mavlink.target_component;
|
||||
vcmd.source_system = msg->sysid;
|
||||
vcmd.source_component = msg->compid;
|
||||
vcmd.confirmation = cmd_mavlink.confirmation;
|
||||
|
||||
/* check if topic is advertised */
|
||||
if (cmd_pub <= 0) {
|
||||
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
}
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
|
||||
mavlink_optical_flow_t flow;
|
||||
mavlink_msg_optical_flow_decode(msg, &flow);
|
||||
|
||||
struct optical_flow_s f;
|
||||
|
||||
f.timestamp = hrt_absolute_time();
|
||||
f.flow_raw_x = flow.flow_x;
|
||||
f.flow_raw_y = flow.flow_y;
|
||||
f.flow_comp_x_m = flow.flow_comp_m_x;
|
||||
f.flow_comp_y_m = flow.flow_comp_m_y;
|
||||
f.ground_distance_m = flow.ground_distance;
|
||||
f.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
|
||||
/* check if topic is advertised */
|
||||
if (flow_pub <= 0) {
|
||||
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||
} else {
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
|
||||
/* Set mode on request */
|
||||
mavlink_set_mode_t new_mode;
|
||||
mavlink_msg_set_mode_decode(msg, &new_mode);
|
||||
|
||||
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
|
||||
vcmd.param1 = new_mode.base_mode;
|
||||
vcmd.param2 = new_mode.custom_mode;
|
||||
vcmd.param3 = 0;
|
||||
vcmd.param4 = 0;
|
||||
vcmd.param5 = 0;
|
||||
vcmd.param6 = 0;
|
||||
vcmd.param7 = 0;
|
||||
vcmd.command = MAV_CMD_DO_SET_MODE;
|
||||
vcmd.target_system = new_mode.target_system;
|
||||
vcmd.target_component = MAV_COMP_ID_ALL;
|
||||
vcmd.source_system = msg->sysid;
|
||||
vcmd.source_component = msg->compid;
|
||||
vcmd.confirmation = 1;
|
||||
|
||||
/* check if topic is advertised */
|
||||
if (cmd_pub <= 0) {
|
||||
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
} else {
|
||||
/* create command */
|
||||
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
|
||||
}
|
||||
}
|
||||
|
||||
/* Handle Vicon position estimates */
|
||||
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
|
||||
mavlink_vicon_position_estimate_t pos;
|
||||
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
|
||||
|
||||
vicon_position.x = pos.x;
|
||||
vicon_position.y = pos.y;
|
||||
vicon_position.z = pos.z;
|
||||
|
||||
if (vicon_position_pub <= 0) {
|
||||
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
|
||||
}
|
||||
}
|
||||
|
||||
/* Handle quadrotor motor setpoints */
|
||||
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
|
||||
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
|
||||
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
|
||||
|
||||
if (mavlink_system.sysid < 4) {
|
||||
|
||||
/* switch to a receiving link mode */
|
||||
gcs_link = false;
|
||||
|
||||
/*
|
||||
* rate control mode - defined by MAVLink
|
||||
*/
|
||||
|
||||
uint8_t ml_mode = 0;
|
||||
bool ml_armed = false;
|
||||
|
||||
switch (quad_motors_setpoint.mode) {
|
||||
case 0:
|
||||
ml_armed = false;
|
||||
break;
|
||||
case 1:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
|
||||
ml_armed = true;
|
||||
|
||||
break;
|
||||
case 2:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
|
||||
ml_armed = true;
|
||||
|
||||
break;
|
||||
case 3:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
|
||||
break;
|
||||
case 4:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
|
||||
break;
|
||||
}
|
||||
|
||||
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
|
||||
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
|
||||
|
||||
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
|
||||
ml_armed = false;
|
||||
}
|
||||
|
||||
offboard_control_sp.armed = ml_armed;
|
||||
offboard_control_sp.mode = ml_mode;
|
||||
|
||||
offboard_control_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
/* check if topic has to be advertised */
|
||||
if (offboard_control_sp_pub <= 0) {
|
||||
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
} else {
|
||||
/* Publish */
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Receive data from UART.
|
||||
*/
|
||||
static void *
|
||||
receive_thread(void *arg)
|
||||
{
|
||||
int uart_fd = *((int*)arg);
|
||||
|
||||
const int timeout = 1000;
|
||||
uint8_t ch;
|
||||
|
||||
mavlink_message_t msg;
|
||||
|
||||
prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
|
||||
|
||||
if (poll(fds, 1, timeout) > 0) {
|
||||
/* non-blocking read until buffer is empty */
|
||||
int nread = 0;
|
||||
|
||||
do {
|
||||
nread = read(uart_fd, &ch, 1);
|
||||
|
||||
if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
|
||||
/* handle generic messages and commands */
|
||||
handle_message(&msg);
|
||||
}
|
||||
} while (nread > 0);
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pthread_t
|
||||
receive_start(int uart)
|
||||
{
|
||||
pthread_attr_t receiveloop_attr;
|
||||
pthread_attr_init(&receiveloop_attr);
|
||||
|
||||
struct sched_param param;
|
||||
param.sched_priority = SCHED_PRIORITY_MAX - 40;
|
||||
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&receiveloop_attr, 2048);
|
||||
|
||||
pthread_t thread;
|
||||
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
|
||||
return thread;
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# MAVLink protocol to uORB interface process (XXX hack for onboard use)
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink_onboard
|
||||
SRCS = mavlink.c \
|
||||
mavlink_receiver.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
@@ -0,0 +1,100 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file orb_topics.h
|
||||
* Common sets of topics subscribed to or published by the MAVLink driver,
|
||||
* and structures maintained by those subscriptions.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#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_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/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
struct mavlink_subscriptions {
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
int global_pos_sub;
|
||||
int act_0_sub;
|
||||
int act_1_sub;
|
||||
int act_2_sub;
|
||||
int act_3_sub;
|
||||
int gps_sub;
|
||||
int man_control_sp_sub;
|
||||
int armed_sub;
|
||||
int actuators_sub;
|
||||
int local_pos_sub;
|
||||
int spa_sub;
|
||||
int spl_sub;
|
||||
int spg_sub;
|
||||
int debug_key_value;
|
||||
int input_rc_sub;
|
||||
};
|
||||
|
||||
extern struct mavlink_subscriptions mavlink_subs;
|
||||
|
||||
/** Global position */
|
||||
extern struct vehicle_global_position_s global_pos;
|
||||
|
||||
/** Local position */
|
||||
extern struct vehicle_local_position_s local_pos;
|
||||
|
||||
/** Vehicle status */
|
||||
// extern struct vehicle_status_s v_status;
|
||||
|
||||
/** RC channels */
|
||||
extern struct rc_channels_s rc;
|
||||
|
||||
/** Actuator armed state */
|
||||
// extern struct actuator_armed_s armed;
|
||||
|
||||
/** Worker thread starter */
|
||||
extern pthread_t uorb_receive_start(void);
|
||||
@@ -0,0 +1,54 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file util.h
|
||||
* Utility and helper functions and data.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "orb_topics.h"
|
||||
|
||||
/** MAVLink communications channel */
|
||||
extern uint8_t chan;
|
||||
|
||||
/** Shutdown marker */
|
||||
extern volatile bool thread_should_exit;
|
||||
|
||||
/**
|
||||
* Translate the custom state into standard mavlink modes and state.
|
||||
*/
|
||||
extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
|
||||
uint8_t *mavlink_state, uint8_t *mavlink_mode);
|
||||
Reference in New Issue
Block a user