aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink_onboard
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink_onboard')
-rw-r--r--src/modules/mavlink_onboard/mavlink.c536
-rw-r--r--src/modules/mavlink_onboard/mavlink_bridge_header.h83
-rw-r--r--src/modules/mavlink_onboard/mavlink_receiver.c344
-rw-r--r--src/modules/mavlink_onboard/module.mk42
-rw-r--r--src/modules/mavlink_onboard/orb_topics.h102
-rw-r--r--src/modules/mavlink_onboard/util.h55
6 files changed, 0 insertions, 1162 deletions
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
deleted file mode 100644
index 0edb53a3e..000000000
--- a/src/modules/mavlink_onboard/mavlink.c
+++ /dev/null
@@ -1,536 +0,0 @@
-/****************************************************************************
- *
- * 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 <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:
- warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud);
- return -EINVAL;
- }
-
- /* open uart */
- warnx("UART is %s, baudrate is %d", 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) {
- warnx("ERROR getting baudrate / termios config for %s: %d", 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) {
- warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state);
- close(uart);
- return -1;
- }
-
-
- if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
- warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", 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_control_mode_s *control_mode, 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 (control_mode->flag_control_manual_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
- }
-
- if (control_mode->flag_system_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;
- }
-
- if (control_mode->flag_control_velocity_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
- } else {
- *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
- }
-
-// 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;
-
- /* XXX this is never written? */
- struct vehicle_status_s v_status;
- struct vehicle_control_mode_s control_mode;
- 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(&control_mode, &armed, &mavlink_state, &mavlink_mode);
-
- /* send heartbeat */
- mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, 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 * 1000.0f,
- v_status.battery_voltage * 1000.0f,
- v_status.battery_current * 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_onboard start [-d <devicename>] [-b <baud rate>]\n"
- " mavlink_onboard stop\n"
- " mavlink_onboard 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, "already running");
-
- thread_should_exit = false;
- mavlink_task = task_spawn_cmd("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;
-}
-
diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h
deleted file mode 100644
index b72bbb2b1..000000000
--- a/src/modules/mavlink_onboard/mavlink_bridge_header.h
+++ /dev/null
@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- * 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);
-
-#include <v1.0/common/mavlink.h>
-
-#endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
deleted file mode 100644
index 4658bcc1d..000000000
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ /dev/null
@@ -1,344 +0,0 @@
-/****************************************************************************
- *
- * 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 <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 */
- warnx("terminating...");
- 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 buf[32];
-
- mavlink_message_t msg;
-
- prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid());
-
- struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
- ssize_t nread = 0;
-
- while (!thread_should_exit) {
- if (poll(fds, 1, timeout) > 0) {
- if (nread < sizeof(buf)) {
- /* to avoid reading very small chunks wait for data before reading */
- usleep(1000);
- }
-
- /* non-blocking read. read may return negative values */
- 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);
- }
- }
- }
- }
-
- 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, &param);
-
- pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
- pthread_t thread;
- pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
- return thread;
-}
diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk
deleted file mode 100644
index a7a4980fa..000000000
--- a/src/modules/mavlink_onboard/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# 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
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
deleted file mode 100644
index 1b49c9ce4..000000000
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- * 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/vehicle_control_mode.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_armed.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 safety_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);
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
deleted file mode 100644
index c84b6fd26..000000000
--- a/src/modules/mavlink_onboard/util.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/****************************************************************************
- *
- * 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_control_mode_s *control_mode, const struct actuator_armed_s *armed,
- uint8_t *mavlink_state, uint8_t *mavlink_mode);