diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 00:10:20 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 00:10:20 +0200 |
commit | c71f4cf8690a707cd7385a90d826d9e0a5a351e2 (patch) | |
tree | bf599fbe2b0f413e7c57bf677684224c322b737a /apps/mavlink | |
parent | 187f7603b9e3ccbf5ac858d250931d583b52d9eb (diff) | |
download | px4-firmware-c71f4cf8690a707cd7385a90d826d9e0a5a351e2.tar.gz px4-firmware-c71f4cf8690a707cd7385a90d826d9e0a5a351e2.tar.bz2 px4-firmware-c71f4cf8690a707cd7385a90d826d9e0a5a351e2.zip |
Cut over MAVLink to new style build system
Diffstat (limited to 'apps/mavlink')
-rw-r--r-- | apps/mavlink/.context | 0 | ||||
-rw-r--r-- | apps/mavlink/.gitignore | 3 | ||||
-rw-r--r-- | apps/mavlink/Makefile | 44 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 825 | ||||
-rw-r--r-- | apps/mavlink/mavlink_bridge_header.h | 81 | ||||
-rw-r--r-- | apps/mavlink/mavlink_hil.h | 61 | ||||
-rw-r--r-- | apps/mavlink/mavlink_log.c | 89 | ||||
-rw-r--r-- | apps/mavlink/mavlink_log.h | 120 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.c | 231 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.h | 104 | ||||
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 718 | ||||
-rw-r--r-- | apps/mavlink/missionlib.c | 200 | ||||
-rw-r--r-- | apps/mavlink/missionlib.h | 52 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 777 | ||||
-rw-r--r-- | apps/mavlink/orb_topics.h | 106 | ||||
-rw-r--r-- | apps/mavlink/util.h | 54 | ||||
-rw-r--r-- | apps/mavlink/waypoints.c | 1134 | ||||
-rw-r--r-- | apps/mavlink/waypoints.h | 131 |
18 files changed, 0 insertions, 4730 deletions
diff --git a/apps/mavlink/.context b/apps/mavlink/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/mavlink/.context +++ /dev/null diff --git a/apps/mavlink/.gitignore b/apps/mavlink/.gitignore deleted file mode 100644 index a02827195..000000000 --- a/apps/mavlink/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -include -mavlink-* -pymavlink-* diff --git a/apps/mavlink/Makefile b/apps/mavlink/Makefile deleted file mode 100644 index 8ddb69b71..000000000 --- a/apps/mavlink/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 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 Application -# - -APPNAME = mavlink -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink - -include $(APPDIR)/mk/app.mk diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c deleted file mode 100644 index 644b779af..000000000 --- a/apps/mavlink/mavlink.c +++ /dev/null @@ -1,825 +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 <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 "waypoints.h" -#include "mavlink_log.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; -} - diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h deleted file mode 100644 index 270ec1727..000000000 --- a/apps/mavlink/mavlink_bridge_header.h +++ /dev/null @@ -1,81 +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); - -#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h deleted file mode 100644 index 8c7a5b514..000000000 --- a/apps/mavlink/mavlink_hil.h +++ /dev/null @@ -1,61 +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 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); diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c deleted file mode 100644 index ed65fb883..000000000 --- a/apps/mavlink/mavlink_log.c +++ /dev/null @@ -1,89 +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_log.c - * MAVLink text logging. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <string.h> -#include <stdlib.h> - -#include "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; - } -} diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h deleted file mode 100644 index 233a76cb3..000000000 --- a/apps/mavlink/mavlink_log.h +++ /dev/null @@ -1,120 +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_log.h - * MAVLink text logging. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef MAVLINK_LOG -#define MAVLINK_LOG - -/* - * IOCTL interface for sending log messages. - */ -#include <sys/ioctl.h> - -/* - * The mavlink log device node; must be opened before messages - * can be logged. - */ -#define MAVLINK_LOG_DEVICE "/dev/mavlink" - -#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1) -#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2) -#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3) - -/** - * Send a mavlink emergency message. - * - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ -#ifdef __cplusplus -#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); -#else -#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); -#endif - -/** - * Send a mavlink critical message. - * - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ -#ifdef __cplusplus -#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); -#else -#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); -#endif - -/** - * Send a mavlink info message. - * - * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); - * @param _text The text to log; - */ -#ifdef __cplusplus -#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); -#else -#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); -#endif - -struct mavlink_logmessage { - char text[51]; - unsigned char severity; -}; - -struct mavlink_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct mavlink_logmessage *elems; -}; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); - -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); - -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); - -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); - -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); - -#endif - diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c deleted file mode 100644 index 819f3441b..000000000 --- a/apps/mavlink/mavlink_parameters.c +++ /dev/null @@ -1,231 +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_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; - } -} diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h deleted file mode 100644 index b1e38bcc8..000000000 --- a/apps/mavlink/mavlink_parameters.h +++ /dev/null @@ -1,104 +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_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); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c deleted file mode 100644 index 22c2fcdad..000000000 --- a/apps/mavlink/mavlink_receiver.c +++ /dev/null @@ -1,718 +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 <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 "waypoints.h" -#include "mavlink_log.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.15; - 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; -} diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c deleted file mode 100644 index 8064febc4..000000000 --- a/apps/mavlink/missionlib.c +++ /dev/null @@ -1,200 +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 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 "waypoints.h" -#include "mavlink_log.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); -} diff --git a/apps/mavlink/missionlib.h b/apps/mavlink/missionlib.h deleted file mode 100644 index c2ca735b3..000000000 --- a/apps/mavlink/missionlib.h +++ /dev/null @@ -1,52 +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 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); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c deleted file mode 100644 index 5f15facf8..000000000 --- a/apps/mavlink/orb_listener.c +++ /dev/null @@ -1,777 +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 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 "waypoints.h" -#include "mavlink_log.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; -} diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h deleted file mode 100644 index d61cd43dc..000000000 --- a/apps/mavlink/orb_topics.h +++ /dev/null @@ -1,106 +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/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); diff --git a/apps/mavlink/util.h b/apps/mavlink/util.h deleted file mode 100644 index a4ff06a88..000000000 --- a/apps/mavlink/util.h +++ /dev/null @@ -1,54 +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 - -/** 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); diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c deleted file mode 100644 index a131b143b..000000000 --- a/apps/mavlink/waypoints.c +++ /dev/null @@ -1,1134 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @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.c - * MAVLink waypoint protocol implementation (BSD-relicensed). - */ - -#include <math.h> -#include <sys/prctl.h> -#include <unistd.h> -#include <stdio.h> - -#include "missionlib.h" -#include "waypoints.h" -#include "util.h" - -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif - -bool debug = false; -bool verbose = false; - - -#define MAVLINK_WPM_NO_PRINTF - -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - -void mavlink_wpm_init(mavlink_wpm_storage *state) -{ - // Set all waypoints to zero - - // Set count to zero - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - -} - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = wpm->current_partner_sysid; - wpa.target_component = wpm->current_partner_compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); - -#endif - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); - } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, - cur->param2, cur->param3, cur->param4, cur->x, - cur->y, cur->z, cur->frame, cur->command); - - wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = wpm->current_partner_sysid; - wpc.target_component = wpm->current_partner_compid; - wpc.count = count; - - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_message_t msg; - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - wp->target_system = wpm->current_partner_sysid; - wp->target_component = wpm->current_partner_compid; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = wpm->current_partner_sysid; - wpr.target_component = wpm->current_partner_compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm->size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm->size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - -/* - * Calculate distance in global frame. - * - * The distance calculation is based on the WGS84 geoid (GPS) - */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) -{ - //TODO: implement for z once altidude contoller is implemented - - static uint16_t counter; - -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; - double x_rad = lat / 180.0 * M_PI; - double y_rad = lon / 180.0 * M_PI; - - double d_lat = x_rad - current_x_rad; - double d_lon = y_rad - current_y_rad; - - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - const double radius_earth = 6371000.0; - - return radius_earth * c; - - } else { - return -1.0f; - } - - counter++; - -} - -/* - * Calculate distance in local frame (NED) - */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - - float dx = (cur->x - x); - float dy = (cur->y - y); - float dz = (cur->z - z); - - return sqrt(dx * dx + dy * dy + dz * dz); - - } else { - return -1.0f; - } -} - -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) -{ - static uint16_t counter; - - // Do not flood the precious wireless link with debug data - // if (wpm->size > 0 && counter % 10 == 0) { - // printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); - // } - - - if (wpm->current_active_wp_id < wpm->size) { - - float orbit = wpm->waypoints[wpm->current_active_wp_id].param2; - int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; - float dist = -1.0f; - - if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); - - } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); - - } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); - - } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { - /* Check if conditions of mission item are satisfied */ - // XXX TODO - } - - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - wpm->pos_reached = true; - - if (counter % 100 == 0) - printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } - } - - //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { - if (wpm->current_active_wp_id < wpm->size) { - mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); - - if (wpm->timestamp_firstinside_orbit == 0) { - // Announce that last waypoint was reached - printf("Reached waypoint %u for the first time \n", cur_wp->seq); - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm->timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) { - printf("Reached waypoint %u long enough \n", cur_wp->seq); - - if (cur_wp->autocontinue) { - cur_wp->current = 0; - - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated restart the waypoint list from the beginning - */ - wpm->current_active_wp_id = 0; - - } else { - if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) - wpm->current_active_wp_id++; - } - - // Fly to next waypoint - wpm->timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->waypoints[wpm->current_active_wp_id].current = true; - wpm->pos_reached = false; - wpm->yaw_reached = false; - printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); - } - } - } - - } else { - wpm->timestamp_lastoutside_orbit = now; - } - - counter++; -} - - -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) -{ - /* check for timed-out operations */ - if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); - -#endif - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_count = 0; - wpm->current_partner_sysid = 0; - wpm->current_partner_compid = 0; - wpm->current_wp_id = -1; - - if (wpm->size == 0) { - wpm->current_active_wp_id = -1; - } - } - - // Do NOT continously send the current WP, since we're not loosing messages - // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // } - - check_waypoints_reached(now, global_position , local_position); - - return OK; -} - - -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) -{ - uint64_t now = mavlink_missionlib_get_system_timestamp(); - - switch (msg->msgid) { -// case MAVLINK_MSG_ID_ATTITUDE: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) -// { -// mavlink_attitude_t att; -// mavlink_msg_attitude_decode(msg, &att); -// float yaw_tolerance = wpm->accept_range_yaw; -// //compare current yaw -// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) -// { -// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) -// wpm->yaw_reached = true; -// } -// else if(att.yaw - yaw_tolerance < 0.0f) -// { -// float lowerBound = 360.0f + att.yaw - yaw_tolerance; -// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) -// wpm->yaw_reached = true; -// } -// else -// { -// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; -// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) -// wpm->yaw_reached = true; -// } -// } -// } -// break; -// } -// -// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// -// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) -// { -// mavlink_local_position_ned_t pos; -// mavlink_msg_local_position_ned_decode(msg, &pos); -// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); -// -// wpm->pos_reached = false; -// -// // compare current position (given in message) with current waypoint -// float orbit = wp->param1; -// -// float dist; -//// if (wp->param2 == 0) -//// { -//// // FIXME segment distance -//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -//// } -//// else -//// { -// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); -//// } -// -// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) -// { -// wpm->pos_reached = true; -// } -// } -// } -// break; -// } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm->size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpm->current_wp_id == wpm->size - 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - -#endif - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_wp_id = 0; - } - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < wpm->size) { - // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - wpm->current_active_wp_id = wpc.seq; - uint32_t i; - - for (i = 0; i < wpm->size; i++) { - if (i == wpm->current_active_wp_id) { - wpm->waypoints[i].current = true; - - } else { - wpm->waypoints[i].current = false; - } - } - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); - -#endif - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->size > 0) { - //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - - } else { - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - - wpm->current_count = wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); - - } else { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); - } - } else { - // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm->current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); - - } else { - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - break; - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - -#endif - } - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); - -#endif - - } else if (wpr.seq >= wpm->size) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); - -#endif - } - } - } - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); - -#endif - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { -// printf("wpc count in: %d\n",wpc.count); -// printf("Comp id: %d\n",msg->compid); -// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); - - if (wpc.count > 0) { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - -#endif - } - - wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - wpm->current_count = wpc.count; - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } - - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (wpc.count == 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("COUNT 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - break; - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CMD"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - -#endif - } - - } else { - if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); - -#endif - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); - -#endif - } - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif - } - - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - mavlink_missionlib_send_gcs_string("GOT WP"); -// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); -// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); - -// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) - if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { - - wpm->timestamp_lastaction = now; - -// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); - -// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { - //mavlink_missionlib_send_gcs_string("DEBUG 2"); - -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); -// - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); -// printf("WP seq: %d\n",wp.seq); - wpm->current_wp_id = wp.seq + 1; - - // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); -// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - -// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - mavlink_missionlib_send_gcs_string("GOT ALL WPS"); - // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - - if (wpm->current_active_wp_id > wpm->rcv_size - 1) { - wpm->current_active_wp_id = wpm->rcv_size - 1; - } - - // switch the waypoints list - // FIXME CHECK!!! - uint32_t i; - - for (i = 0; i < wpm->current_count; ++i) { - wpm->waypoints[i] = wpm->rcv_waypoints[i]; - } - - wpm->size = wpm->current_count; - - //get the new current waypoint - - for (i = 0; i < wpm->size; i++) { - if (wpm->waypoints[i].current == 1) { - wpm->current_active_wp_id = i; - //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == wpm->size) { - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - wpm->timestamp_firstinside_orbit = 0; - } - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - } else { - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - } - - } else { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); - break; - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (!(wp.seq == 0)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (!(wp.seq == wpm->current_wp_id)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (!(wp.seq < wpm->current_count)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - } - } else { - //we we're target but already communicating with someone else - if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); - } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; - - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm->size = 0; - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); - } - - break; - } - - default: { - // if (debug) // printf("Waypoint: received message of unknown type"); - break; - } - } - - check_waypoints_reached(now, global_pos, local_pos); -} diff --git a/apps/mavlink/waypoints.h b/apps/mavlink/waypoints.h deleted file mode 100644 index c32ab32e5..000000000 --- a/apps/mavlink/waypoints.h +++ /dev/null @@ -1,131 +0,0 @@ -/**************************************************************************** - * - * 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_ */ |