diff options
38 files changed, 4814 insertions, 4536 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0cd8a0e04..0de2d4295 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,38 +5,7 @@ echo "Starting MAVLink on this USB console" -# Stop tone alarm -tone_alarm stop - -# -# Check for UORB -# -if uorb start -then - echo "uORB started" -fi - -# Tell MAVLink that this link is "fast" -if mavlink stop -then - echo "stopped other MAVLink instance" -fi -mavlink start -b 230400 -d /dev/ttyACM0 - -# Stop commander -if commander stop -then - echo "Commander stopped" -fi -sleep 1 - -# Start the commander -if commander start -then - echo "Commander started" -fi - -echo "MAVLink started, exiting shell.." +mavlink start -r 5000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 57299d772..eba18ddb1 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -117,6 +117,8 @@ then set PWM_MAX none set MKBLCTRL_MODE none set FMU_MODE pwm + set MAVLINK_FLAGS default + set EXIT_ON_END no set MAV_TYPE none # @@ -384,28 +386,34 @@ then # # MAVLink # - set EXIT_ON_END no - if [ $HIL == yes ] + if [ $MAVLINK_FLAGS == default ] then - sleep 1 - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - else - if [ $TTYS1_BUSY == yes ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - mavlink start -d /dev/ttyS0 + sleep 1 + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 + # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s + if [ $TTYS1_BUSY == yes ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + set MAVLINK_FLAGS "-r 1000" + usleep 5000 + fi fi fi + + mavlink start $MAVLINK_FLAGS + usleep 5000 # # Start the datamanager @@ -544,6 +552,7 @@ then if [ $EXIT_ON_END == yes ] then + echo "[init] Exit from nsh" exit fi diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..412d0c9e1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,6 @@ MODULES += systemcmds/hw_ver MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink -MODULES += modules/mavlink_onboard # # Estimation modules (EKF/ SO3 / other filters) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 20edc68aa..00bf83bd5 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=36 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 26222c255..9c75e6c59 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=36 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 @@ -796,11 +796,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=4000 +CONFIG_SCHED_WORKSTACKSIZE=2000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORKSTACKSIZE=2000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 5054937e0..0ea655cac 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); */ #define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); + struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; unsigned char severity; @@ -112,6 +113,7 @@ struct mavlink_logbuffer { struct mavlink_logmessage *elems; }; +__BEGIN_DECLS void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); @@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...); +__END_DECLS #endif diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9b3e202e6..59c04f0e3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -441,14 +441,14 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; - while (bearing > M_PI_F) { + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; if (c++ > 3) return NAN; } c = 0; - while (bearing <= -M_PI_F) { + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; if (c++ > 3) return NAN; @@ -465,14 +465,14 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; - while (bearing > M_TWOPI_F) { + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; if (c++ > 3) return NAN; } c = 0; - while (bearing <= 0.0f) { + while (bearing < 0.0f) { bearing += M_TWOPI_F; if (c++ > 3) return NAN; @@ -489,14 +489,14 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; - while (bearing > 180.0f) { + while (bearing >= 180.0f) { bearing -= 360.0f; if (c++ > 3) return NAN; } c = 0; - while (bearing <= -180.0f) { + while (bearing < -180.0f) { bearing += 360.0f; if (c++ > 3) return NAN; @@ -513,14 +513,14 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; - while (bearing > 360.0f) { + while (bearing >= 360.0f) { bearing -= 360.0f; if (c++ > 3) return NAN; } c = 0; - while (bearing <= 0.0f) { + while (bearing < 0.0f) { bearing += 360.0f; if (c++ > 3) return NAN; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..2144d3460 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,6 +8,8 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ +#include <stdint.h> + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ade4469c5..542bf0d7e 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -34,46 +33,43 @@ /** * @file mavlink.c - * MAVLink 1.0 protocol implementation. + * Adapter functions expected by the protocol library * * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> #include <unistd.h> -#include <pthread.h> #include <stdio.h> -#include <math.h> #include <stdbool.h> -#include <fcntl.h> -#include <mqueue.h> #include <string.h> #include "mavlink_bridge_header.h" -#include <drivers/drv_hrt.h> -#include <time.h> -#include <float.h> -#include <unistd.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <stdlib.h> -#include <poll.h> - #include <systemlib/param/param.h> -#include <systemlib/systemlib.h> -#include <systemlib/err.h> -#include <mavlink/mavlink_log.h> -#include <commander/px4_custom_mode.h> - -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" - -#include <uORB/topics/mission_result.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/systemlib.h> +// #include <systemlib/err.h> +// #include <mavlink/mavlink_log.h> +// #include <commander/px4_custom_mode.h> + +// #include "waypoints.h" +// #include "orb_topics.h" +// #include "mavlink_hil.h" +// #include "util.h" +// #include "waypoints.h" +// #include "mavlink_parameters.h" + +// #include <uORB/topics/mission_result.h> /* define MAVLink specific parameters */ /** @@ -92,22 +88,6 @@ 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, @@ -117,362 +97,6 @@ mavlink_system_t mavlink_system = { 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) { - - 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_base_mode, uint32_t *mavlink_custom_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_base_mode = 0; - *mavlink_custom_mode = 0; - - /** - * Set mode flags - **/ - - /* HIL */ - if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* arming state */ - if (armed.armed) { - *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - /* main state */ - *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } - } - *mavlink_custom_mode = custom_mode.data; - - /** - * Set mavlink state - **/ - - /* set calibration state */ - if (v_status.arming_state == ARMING_STATE_INIT - || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE - || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - *mavlink_state = MAV_STATE_UNINIT; - } else if (v_status.arming_state == ARMING_STATE_ARMED) { - *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_state = MAV_STATE_CRITICAL; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { - *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_REBOOT) { - *mavlink_state = MAV_STATE_POWEROFF; - } else { - warnx("Unknown mavlink state"); - *mavlink_state = MAV_STATE_CRITICAL; - } -} - - -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); - orb_set_interval(subs->actuators_effective_sub, min_interval); - orb_set_interval(subs->spa_sub, min_interval); - orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); - 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: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); - return -EINVAL; - } - - /* open uart */ - warnx("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; - - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %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; - - /* USB serial is indicated by /dev/ttyACM0*/ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); - close(uart); - return -1; - } - - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - return uart; -} - -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const 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 */ @@ -490,362 +114,3 @@ extern 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, 10); - - 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 < 9600 || baudrate > 921600) - 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(); - break; - } - } - - 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); - } - - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); - - 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_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); - - /* switch HIL mode if required */ - if (v_status.hil_state == HIL_STATE_ON) - set_hil_on_off(true); - else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); - - /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, - v_status.onboard_control_sensors_present, - v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, - v_status.load * 1000.0f, - v_status.battery_voltage * 1000.0f, - v_status.battery_current * 100.0f, - v_status.battery_remaining * 100.0f, - 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++; - - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); - } - } - - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* sleep quarter the time */ - usleep(25000); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* sleep quarter the time */ - usleep(25000); - - /* send parameters at 20 Hz (if queued for sending) */ - mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* sleep quarter the time */ - usleep(25000); - - mavlink_waypoint_eventloop(hrt_absolute_time()); - - 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 */ - tcsetattr(uart, TCSANOW, &uart_config_original); - - /* destroy log buffer */ - //mavlink_logbuffer_destroy(&lb); - - thread_running = false; - - return 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"); - - thread_should_exit = false; - mavlink_task = task_spawn_cmd("mavlink", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - mavlink_thread_main, - (const char **)argv); - - while (!thread_running) { - usleep(200); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - /* this is not an error */ - if (!thread_running) - errx(0, "mavlink already stopped"); - - thread_should_exit = true; - - while (thread_running) { - usleep(200000); - warnx("."); - } - - warnx("terminated."); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -} - diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 149efda60..374d1511c 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -43,6 +42,8 @@ #ifndef MAVLINK_BRIDGE_HEADER_H #define MAVLINK_BRIDGE_HEADER_H +__BEGIN_DECLS + #define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ @@ -73,11 +74,13 @@ extern mavlink_system_t mavlink_system; * @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, const uint8_t *ch, int length); +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); #include <v1.0/common/mavlink.h> +__END_DECLS + #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp new file mode 100644 index 000000000..788fe5732 --- /dev/null +++ b/src/modules/mavlink/mavlink_main.cpp @@ -0,0 +1,1992 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main.cpp + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <assert.h> +#include <math.h> +#include <poll.h> +#include <termios.h> +#include <time.h> +#include <math.h> /* isinf / isnan checks */ + +#include <sys/ioctl.h> +#include <sys/types.h> +#include <sys/stat.h> + +#include <drivers/device/device.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> + +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <geo/geo.h> +#include <dataman/dataman.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> + +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +#include "mavlink_bridge_header.h" +#include "mavlink_main.h" +#include "mavlink_messages.h" +#include "mavlink_receiver.h" +#include "mavlink_rate_limiter.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define DEFAULT_DEVICE_NAME "/dev/ttyS1" +#define MAX_DATA_RATE 10000 // max data rate in bytes/s +#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate + +static Mavlink *_mavlink_instances = nullptr; + +/* TODO: if this is a class member it crashes */ +static struct file_operations fops; + +/** + * mavlink app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); + +/* + * Internal function to send the bytes through the right serial port + */ +void +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) +{ + int uart = -1; + + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); + break; + + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); + break; + + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); + break; + + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); + break; +#ifdef MAVLINK_COMM_4 + + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); + break; +#endif +#ifdef MAVLINK_COMM_5 + + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); + break; +#endif +#ifdef MAVLINK_COMM_6 + + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); + break; +#endif + } + + ssize_t desired = (sizeof(uint8_t) * length); + ssize_t ret = write(uart, ch, desired); + + if (ret != desired) { + warn("write err"); + } + +} + +static void usage(void); + +Mavlink::Mavlink() : + next(nullptr), + _device_name(DEFAULT_DEVICE_NAME), + _task_should_exit(false), + _mavlink_fd(-1), + _task_running(false), + _mavlink_hil_enabled(false), + _main_loop_delay(1000), + _subscriptions(nullptr), + _streams(nullptr), + _mission_pub(-1), + _mavlink_param_queue_index(0), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) +{ + _wpm = &_wpm_s; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; +} + +Mavlink::~Mavlink() +{ + if (_task_running) { + /* task wakes up every 10ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); + break; + } + } while (_task_running); + } +} + +void +Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + +int +Mavlink::instance_count() +{ + unsigned inst_index = 0; + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + inst_index++; + } + + return inst_index; +} + +Mavlink * +Mavlink::new_instance() +{ + Mavlink *inst = new Mavlink(); + Mavlink *next = ::_mavlink_instances; + + /* create the first instance at _head */ + if (::_mavlink_instances == nullptr) { + ::_mavlink_instances = inst; + /* afterwards follow the next and append the instance */ + + } else { + while (next->next != nullptr) { + next = next->next; + } + + /* now parent has a null pointer, fill it */ + next->next = inst; + } + + return inst; +} + +Mavlink * +Mavlink::get_instance(unsigned instance) +{ + Mavlink *inst; + unsigned inst_index = 0; + LL_FOREACH(::_mavlink_instances, inst) { + if (instance == inst_index) { + return inst; + } + + inst_index++; + } + + return nullptr; +} + +Mavlink * +Mavlink::get_instance_for_device(const char *device_name) +{ + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + if (strcmp(inst->_device_name, device_name) == 0) { + return inst; + } + } + + return nullptr; +} + +int +Mavlink::destroy_all_instances() +{ + /* start deleting from the end */ + Mavlink *inst_to_del = nullptr; + Mavlink *next_inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (next_inst != nullptr) { + inst_to_del = next_inst; + next_inst = inst_to_del->next; + + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; + + while (inst_to_del->_task_running) { + printf("."); + fflush(stdout); + usleep(10000); + iterations++; + + if (iterations > 1000) { + warnx("ERROR: Couldn't stop all mavlink instances."); + return ERROR; + } + } + + delete inst_to_del; + } + + /* reset head */ + ::_mavlink_instances = nullptr; + + printf("\n"); + warnx("all instances stopped"); + return OK; +} + +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + + /* don't compare with itself */ + if (inst != self && !strcmp(device_name, inst->_device_name)) { + return true; + } + + inst = inst->next; + } + + return false; +} + +int +Mavlink::get_uart_fd(unsigned index) +{ + Mavlink *inst = get_instance(index); + + if (inst) { + return inst->get_uart_fd(); + } + + return -1; +} + +int +Mavlink::get_uart_fd() +{ + return _uart_fd; +} + +mavlink_channel_t +Mavlink::get_channel() +{ + return _channel; +} + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +int +Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + 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; +// printf("logmsg: %s\n", txt); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; + inst = inst->next; + + } + + return OK; + } + + default: + return ENOTTY; + } +} + +void Mavlink::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; + } +} + +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + return -EINVAL; + } + + /* open uart */ + _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(_uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(_uart_fd); + return -1; + } + + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(_uart_fd); + return -1; + } + + return _uart_fd; +} + +int +Mavlink::set_hil_enabled(bool hil_enabled) +{ + int ret = OK; + + /* Enable HIL */ + if (hil_enabled && !_mavlink_hil_enabled) { + + _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(subs.spa_sub, hil_rate_interval); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + } + + if (!hil_enabled && _mavlink_hil_enabled) { + _mavlink_hil_enabled = false; +// orb_set_interval(subs.spa_sub, 200); + + } else { + ret = ERROR; + } + + return ret; +} + +extern mavlink_system_t mavlink_system; + +int Mavlink::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::mavlink_pm_start_queued_send() +{ + _mavlink_param_queue_index = 0; +} + +int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) +{ + return mavlink_pm_send_param(param_for_index(index)); +} + +int Mavlink::mavlink_pm_send_param_for_name(const char *name) +{ + return mavlink_pm_send_param(param_find(name)); +} + +int Mavlink::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)); + mavlink_missionlib_send_message(&tx_msg); + return OK; +} + +void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* 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; + } +} + +void Mavlink::publish_mission() +{ + /* Initialize mission publication if necessary */ + if (_mission_pub < 0) { + _mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), _mission_pub, &mission); + } +} + +int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param2; + break; + + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + + mission_item->time_inside = mavlink_mission_item->param1; + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + return OK; +} + +int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param2 = mission_item->pitch_min; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} + +void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) +{ + 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->current_dataman_id = 0; +} + +/* + * @brief Sends an waypoint ack message + */ +void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); + mavlink_missionlib_send_message(&msg); + + if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } +} + +/* + * @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::mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if (seq < _wpm->size) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + } else if (seq == 0 && _wpm->size == 0) { + + /* don't broadcast if no WPs */ + + } else { + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + + if (_verbose) { warnx("ERROR: index out of bounds"); } + } +} + +void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = mission.count; + + mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } +} + +void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + + struct mission_item_s mission_item; + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_current; + + if (_wpm->current_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + if (dm_read(dm_current, seq, &mission_item, len) == len) { + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + map_mission_item_to_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); + mavlink_missionlib_send_message(&msg); + + if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + + if (_verbose) { warnx("ERROR: could not read WP%u", seq); } + } +} + +void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _wpm->max_size) { + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); + mavlink_missionlib_send_message(&msg); + + if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } + } +} + +/* + * @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::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 (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } +} + +void Mavlink::mavlink_waypoint_eventloop(uint64_t now) +{ + /* check for timed-out operations */ + if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + + mavlink_missionlib_send_gcs_string("Operation timeout"); + + if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } + + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_partner_sysid = 0; + _wpm->current_partner_compid = 0; + } +} + + +void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) +{ + uint64_t now = hrt_absolute_time(); + + switch (msg->msgid) { + + 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) { + + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; + } + } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); + + if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } + } + + 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) { + + mission.current_index = wpc.seq; + publish_mission(); + + /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ +// mavlink_wpm_send_waypoint_current(wpc.seq); + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + + if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } + } + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); + + if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } + + } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + + if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } + } + + 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) { + + _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) { warnx("No waypoints send"); } + } + + _wpm->current_count = _wpm->size; + mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); + + } else { + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + + if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } + } + + } else { + mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); + + if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } + } + + 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; + + if (wpr.seq >= _wpm->size) { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } + + break; + } + + /* + * 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) { + + if (wpr.seq == 0) { + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + + if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } + + break; + } + + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + + if (wpr.seq == _wpm->current_wp_id) { + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + + } else if (wpr.seq == _wpm->current_wp_id + 1) { + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } + + break; + } + + } else { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } + + break; + } + + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + + if (wpr.seq < _wpm->size) { + + mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + + if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } + } + + + } 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)) { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } + + } else { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } + } + } + + 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) { + + if (wpc.count > NUM_MISSIONS_SUPPORTED) { + if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); + break; + } + + if (wpc.count == 0) { + mavlink_missionlib_send_gcs_string("COUNT 0"); + + if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + break; + } + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _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; + + 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_GETLIST) { + + if (_wpm->current_wp_id == 0) { + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + } + + } else { + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + + if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } + } + + } else { + + mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); + + if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } + } + } + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { + + _wpm->timestamp_lastaction = now; + + /* + * 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) { + + if (wp.seq != 0) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + break; + } + + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + + if (wp.seq >= _wpm->current_count) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); + break; + } + + if (wp.seq != _wpm->current_wp_id) { + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); + break; + } + } + + _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + + struct mission_item_s mission_item; + + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); + + if (ret != OK) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_next; + + if (_wpm->current_dataman_id == 0) { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; + mission.dataman_id = 1; + + } else { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.dataman_id = 0; + } + + if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + +// if (wp.current) { +// warnx("current is: %d", wp.seq); +// mission.current_index = wp.seq; +// } + // XXX ignore current set + mission.current_index = -1; + + _wpm->current_wp_id = wp.seq + 1; + + if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + + if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } + + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + mission.count = _wpm->current_count; + + publish_mission(); + + _wpm->current_dataman_id = mission.dataman_id; + _wpm->size = _wpm->current_count; + + _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 { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } + } + + 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 */) { + + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + _wpm->timestamp_lastaction = now; + + _wpm->size = 0; + + /* prepare mission topic */ + mission.dataman_id = -1; + mission.count = 0; + mission.current_index = -1; + publish_mission(); + + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + } + + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } + } + + + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + + mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); + + if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } + } + + break; + } + + default: { + /* other messages might should get caught by mavlink and others */ + break; + } + } +} + +void +Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + + mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); +} + + + +int +Mavlink::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); + mavlink_missionlib_send_message(&msg); + return OK; + + } else { + return 1; + } +} + +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) +{ + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; + + LL_FOREACH(_subscriptions, sub) { + if (sub->get_topic() == topic) { + /* already subscribed */ + return sub; + } + } + + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; +} + +int +Mavlink::configure_stream(const char *stream_name, const float rate) +{ + /* calculate interval in us, 0 means disabled stream */ + unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (strcmp(stream_name, stream->get_name()) == 0) { + if (interval > 0) { + /* set new interval */ + stream->set_interval(interval); + + } else { + /* delete stream */ + LL_DELETE(_streams, stream); + delete stream; + } + + return OK; + } + } + + if (interval > 0) { + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(interval); + stream->subscribe(this); + LL_APPEND(_streams, stream); + return OK; + } + } + + } else { + /* stream not found, nothing to disable */ + return OK; + } + + return ERROR; +} + +void +Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) +{ + /* orb subscription must be done from the main thread, + * set _subscribe_to_stream and _subscribe_to_stream_rate fields + * which polled in mavlink main loop */ + if (!_task_should_exit) { + /* wait for previous subscription completion */ + while (_subscribe_to_stream != nullptr) { + usleep(MAIN_LOOP_DELAY / 2); + } + + /* copy stream name */ + unsigned n = strlen(stream_name) + 1; + char *s = new char[n]; + memcpy(s, stream_name, n); + + /* set subscription task */ + _subscribe_to_stream_rate = rate; + _subscribe_to_stream = s; + + /* wait for subscription */ + do { + usleep(MAIN_LOOP_DELAY / 2); + } while (_subscribe_to_stream != nullptr); + } +} + +int +Mavlink::task_main(int argc, char *argv[]) +{ + /* inform about start */ + warnx("start"); + fflush(stdout); + + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&_logbuffer, 5); + + int ch; + _baudrate = 57600; + _datarate = 0; + _channel = MAVLINK_COMM_0; + + _mode = MODE_OFFBOARD; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + switch (ch) { + case 'b': + _baudrate = strtoul(optarg, NULL, 10); + + if (_baudrate < 9600 || _baudrate > 921600) { + warnx("invalid baud rate '%s'", optarg); + err_flag = true; + } + + break; + + case 'r': + _datarate = strtoul(optarg, NULL, 10); + + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { + warnx("invalid data rate '%s'", optarg); + err_flag = true; + } + + break; + + case 'd': + _device_name = optarg; + break; + +// case 'e': +// mavlink_link_termination_allowed = true; +// break; + + case 'm': + if (strcmp(optarg, "offboard") == 0) { + _mode = MODE_OFFBOARD; + + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MODE_ONBOARD; + + } else if (strcmp(optarg, "hil") == 0) { + _mode = MODE_HIL; + + } else if (strcmp(optarg, "custom") == 0) { + _mode = MODE_CUSTOM; + } + + break; + + case 'v': + _verbose = true; + break; + + default: + err_flag = true; + break; + } + } + + if (err_flag) { + usage(); + exit(1); + } + + if (_datarate == 0) { + /* convert bits to bytes and use 1/2 of bandwidth by default */ + _datarate = _baudrate / 20; + } + + if (_datarate > MAX_DATA_RATE) { + _datarate = MAX_DATA_RATE; + } + + if (Mavlink::instance_exists(_device_name, this)) { + errx(1, "mavlink instance for %s already running", _device_name); + } + + /* inform about mode */ + switch (_mode) { + case MODE_CUSTOM: + warnx("mode: CUSTOM"); + break; + + case MODE_OFFBOARD: + warnx("mode: OFFBOARD"); + break; + + case MODE_ONBOARD: + warnx("mode: ONBOARD"); + break; + + case MODE_HIL: + warnx("mode: HIL"); + break; + + default: + warnx("ERROR: Unknown mode"); + break; + } + + switch (_mode) { + case MODE_OFFBOARD: + case MODE_HIL: + case MODE_CUSTOM: + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + break; + + case MODE_ONBOARD: + _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; + break; + + default: + _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; + break; + } + + warnx("data rate: %d bytes/s", _datarate); + warnx("port: %s, baudrate: %d", _device_name, _baudrate); + + /* flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + struct termios uart_config_original; + bool usb_uart; + + /* default values for arguments */ + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart); + + if (_uart_fd < 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, &fops, 0666, NULL); + + /* initialize logging device */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + _receive_thread = MavlinkReceiver::receive_start(this); + + /* initialize waypoint manager */ + mavlink_wpm_init(_wpm); + + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + + _task_running = true; + + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); + + struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + + warnx("started"); + mavlink_log_info(_mavlink_fd, "[mavlink] started"); + + /* add default streams depending on mode and intervals depending on datarate */ + float rate_mult = _datarate / 1000.0f; + + configure_stream("HEARTBEAT", 1.0f); + + switch (_mode) { + case MODE_OFFBOARD: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("HIGHRES_IMU", 1.0f * rate_mult); + configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("GPS_RAW_INT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); + configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); + break; + + case MODE_HIL: + /* HIL mode normally runs at high data rate, rate_mult ~ 10 */ + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("HIGHRES_IMU", 1.0f * rate_mult); + configure_stream("ATTITUDE", 2.0f * rate_mult); + configure_stream("VFR_HUD", 2.0f * rate_mult); + configure_stream("GPS_RAW_INT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); + configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); + break; + + default: + break; + } + + /* don't send parameters on startup without request */ + _mavlink_param_queue_index = param_count(); + + MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(100000.0f / rate_mult); + + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + + while (!_task_should_exit) { + /* main loop */ + usleep(_main_loop_delay); + + perf_begin(_loop_perf); + + hrt_abstime t = hrt_absolute_time(); + + if (param_sub->update(t)) { + /* parameters updated */ + mavlink_update_system(); + } + + if (status_sub->update(t)) { + /* switch HIL mode if required */ + if (status->hil_state == HIL_STATE_ON) { + set_hil_enabled(true); + + } else if (status->hil_state == HIL_STATE_OFF) { + set_hil_enabled(false); + } + } + + /* check for requested subscriptions */ + if (_subscribe_to_stream != nullptr) { + if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { + if (_subscribe_to_stream_rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); + + } else { + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + } + + } else { + warnx("stream %s not found", _subscribe_to_stream, _device_name); + } + + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + } + + /* update streams */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); + } + + bool updated; + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } + + if (mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + } + + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + + } else { + if (slow_rate_limiter.check(t)) { + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } + } + + if (fast_rate_limiter.check(t)) { + mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(hrt_absolute_time()); + + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); + + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } + } + + perf_end(_loop_perf); + } + + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + + warnx("waiting for UART receive thread"); + + /* wait for threads to complete */ + pthread_join(_receive_thread, NULL); + + /* reset the UART flags to original state */ + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); + + /* close UART */ + close(_uart_fd); + + /* destroy log buffer */ + mavlink_logbuffer_destroy(&_logbuffer); + + warnx("exiting"); + + _task_running = false; + _exit(0); +} + +int Mavlink::start_helper(int argc, char *argv[]) +{ + /* create the instance in task context */ + Mavlink *instance = Mavlink::new_instance(); + + /* this will actually only return once MAVLink exits */ + return instance->task_main(argc, argv); +} + +int +Mavlink::start(int argc, char *argv[]) +{ + // Instantiate thread + char buf[32]; + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + + task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + return OK; +} + +void +Mavlink::status() +{ + warnx("running"); +} + +int +Mavlink::stream(int argc, char *argv[]) +{ + const char *device_name = DEFAULT_DEVICE_NAME; + float rate = -1.0f; + const char *stream_name = nullptr; + int ch; + + argc -= 1; + argv += 1; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + while ((ch = getopt(argc, argv, "r:d:s:")) != EOF) { + switch (ch) { + case 'r': + rate = strtod(optarg, nullptr); + + if (rate < 0.0f) { + err_flag = true; + } + + break; + + case 'd': + device_name = optarg; + break; + + case 's': + stream_name = optarg; + break; + + default: + err_flag = true; + break; + } + } + + if (!err_flag && rate >= 0.0 && stream_name != nullptr) { + Mavlink *inst = get_instance_for_device(device_name); + + if (inst != nullptr) { + inst->configure_stream_threadsafe(stream_name, rate); + + } else { + errx(1, "mavlink for device %s is not running", device_name); + } + + } else { + errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + } + + return OK; +} + +static void usage() +{ + errx(1, "usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); +} + +int mavlink_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + return Mavlink::start(argc, argv); + + } else if (!strcmp(argv[1], "stop")) { + warnx("mavlink stop is deprecated, use stop-all instead"); + usage(); + + } else if (!strcmp(argv[1], "stop-all")) { + return Mavlink::destroy_all_instances(); + + // } else if (!strcmp(argv[1], "status")) { + // mavlink::g_mavlink->status(); + + } else if (!strcmp(argv[1], "stream")) { + return Mavlink::stream(argc, argv); + + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h new file mode 100644 index 000000000..b52c12796 --- /dev/null +++ b/src/modules/mavlink/mavlink_main.h @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#pragma once + +#include <stdbool.h> +#include <nuttx/fs/fs.h> +#include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> +#include <pthread.h> +#include <mavlink/mavlink_log.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> + +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" +#include "mavlink_messages.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 +}; + + +#define MAVLINK_WPM_MAX_WP_COUNT 255 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + uint16_t size; + uint16_t max_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint32_t timeout; + int current_dataman_id; +}; + + +class Mavlink +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void status(); + + static int stream(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); + + static int destroy_all_instances(); + + static bool instance_exists(const char *device_name, Mavlink *self); + + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + const char *_device_name; + + enum MAVLINK_MODE { + MODE_CUSTOM = 0, + MODE_OFFBOARD, + MODE_ONBOARD, + MODE_HIL + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _mavlink_hil_enabled; }; + + /** + * Handle waypoint related messages. + */ + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + + static int start_helper(int argc, char *argv[]); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * 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. + */ + int set_hil_enabled(bool hil_enabled); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + + mavlink_channel_t get_channel(); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + +protected: + Mavlink *next; + +private: + int _mavlink_fd; + bool _task_running; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /* states */ + bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; + + orb_advert_t _mission_pub; + struct mission_s mission; + uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; + MAVLINK_MODE _mode; + + uint8_t _mavlink_wpm_comp_id; + mavlink_channel_t _channel; + + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; + + pthread_t _receive_thread; + + /* Allocate storage space for waypoints */ + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; + + bool _verbose; + int _uart_fd; + int _baudrate; + int _datarate; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + /** + * 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 mavlink_update_system(); + + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); + void mavlink_wpm_send_waypoint_current(uint16_t seq); + void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); + void mavlink_wpm_init(mavlink_wpm_storage *state); + int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + void publish_mission(); + + void mavlink_missionlib_send_message(mavlink_message_t *msg); + int mavlink_missionlib_send_gcs_string(const char *string); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + int configure_stream(const char *stream_name, const float rate); + void configure_stream_threadsafe(const char *stream_name, const float rate); + + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]) __attribute__((noreturn)); + +}; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp new file mode 100644 index 000000000..4d83afe82 --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -0,0 +1,1196 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_messages.cpp + * MAVLink 1.0 message formatters implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdio.h> +#include <commander/px4_custom_mode.h> +#include <lib/geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/telemetry_status.h> +#include <uORB/topics/debug_key_value.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/navigation_capabilities.h> +#include <drivers/drv_rc_input.h> + +#include "mavlink_messages.h" + + +static uint16_t cm_uint16_from_m_float(float m); +static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + +void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +{ + *mavlink_state = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + union px4_custom_mode custom_mode; + custom_mode.data = 0; + + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + + } else if (status->main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + + } else if (status->main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + + } else if (status->main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + + } else { + /* use navigation state when navigator is active */ + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } + } + + *mavlink_custom_mode = custom_mode.data; + + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + + } else if (status->arming_state == ARMING_STATE_ARMED) { + *mavlink_state = MAV_STATE_ACTIVE; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + + } else if (status->arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; + + } else { + *mavlink_state = MAV_STATE_CRITICAL; + } +} + + +class MavlinkStreamHeartbeat : public MavlinkStream +{ +public: + const char *get_name() + { + return "HEARTBEAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHeartbeat(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + } + + void send(const hrt_abstime t) + { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); + + } +}; + + +class MavlinkStreamSysStatus : public MavlinkStream +{ +public: + const char *get_name() + { + return "SYS_STATUS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamSysStatus(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + } + + void send(const hrt_abstime t) + { + status_sub->update(t); + + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } +}; + + +class MavlinkStreamHighresIMU : public MavlinkStream +{ +public: + MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + { + } + + const char *get_name() + { + return "HIGHRES_IMU"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHighresIMU(); + } + +private: + MavlinkOrbSubscription *sensor_sub; + struct sensor_combined_s *sensor; + + uint32_t accel_counter; + uint32_t gyro_counter; + uint32_t mag_counter; + uint32_t baro_counter; + +protected: + void subscribe(Mavlink *mavlink) + { + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); + sensor = (struct sensor_combined_s *)sensor_sub->get_data(); + } + + void send(const hrt_abstime t) + { + sensor_sub->update(t); + + uint16_t fields_updated = 0; + + if (accel_counter != sensor->accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = sensor->accelerometer_counter; + } + + if (gyro_counter != sensor->gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = sensor->gyro_counter; + } + + if (mag_counter != sensor->magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = sensor->magnetometer_counter; + } + + if (baro_counter != sensor->baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = sensor->baro_counter; + } + + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); + } +}; + + +class MavlinkStreamAttitude : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitude(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) + { + att_sub->update(t); + + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } +}; + + +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE_QUATERNION"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeQuaternion(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) + { + att_sub->update(t); + + mavlink_msg_attitude_quaternion_send(_channel, + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); + } +}; + + +class MavlinkStreamVFRHUD : public MavlinkStream +{ +public: + const char *get_name() + { + return "VFR_HUD"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamVFRHUD(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + + MavlinkOrbSubscription *pos_sub; + struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *armed_sub; + struct actuator_armed_s *armed; + + MavlinkOrbSubscription *act_sub; + struct actuator_controls_s *act; + + MavlinkOrbSubscription *airspeed_sub; + struct airspeed_s *airspeed; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); + armed = (struct actuator_armed_s *)armed_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + act = (struct actuator_controls_s *)act_sub->get_data(); + + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); + airspeed = (struct airspeed_s *)airspeed_sub->get_data(); + } + + void send(const hrt_abstime t) + { + att_sub->update(t); + pos_sub->update(t); + armed_sub->update(t); + act_sub->update(t); + airspeed_sub->update(t); + + float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); + uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; + float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + + mavlink_msg_vfr_hud_send(_channel, + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); + } +}; + + +class MavlinkStreamGPSRawInt : public MavlinkStream +{ +public: + const char *get_name() + { + return "GPS_RAW_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGPSRawInt(); + } + +private: + MavlinkOrbSubscription *gps_sub; + struct vehicle_gps_position_s *gps; + +protected: + void subscribe(Mavlink *mavlink) + { + gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); + gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); + } + + void send(const hrt_abstime t) + { + gps_sub->update(t); + + mavlink_msg_gps_raw_int_send(_channel, + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); + } +}; + + +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ +public: + const char *get_name() + { + return "GLOBAL_POSITION_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionInt(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *home_sub; + struct home_position_s *home; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) + { + pos_sub->update(t); + home_sub->update(t); + + mavlink_msg_global_position_int_send(_channel, + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } +}; + + +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ +public: + const char *get_name() + { + return "LOCAL_POSITION_NED"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionNED(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_local_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); + pos = (struct vehicle_local_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + pos_sub->update(t); + + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } +}; + + +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ +public: + const char *get_name() + { + return "GPS_GLOBAL_ORIGIN"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGPSGlobalOrigin(); + } + +private: + MavlinkOrbSubscription *home_sub; + struct home_position_s *home; + +protected: + void subscribe(Mavlink *mavlink) + { + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) + { + home_sub->update(t); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); + } +}; + + +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ +public: + MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) + { + sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); + } + + const char *get_name() + { + return _name; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamServoOutputRaw(_n); + } + +private: + MavlinkOrbSubscription *act_sub; + struct actuator_outputs_s *act; + + char _name[20]; + unsigned int _n; + +protected: + void subscribe(Mavlink *mavlink) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + act_sub = mavlink->add_orb_subscription(act_topics[_n]); + act = (struct actuator_outputs_s *)act_sub->get_data(); + } + + void send(const hrt_abstime t) + { + act_sub->update(t); + + mavlink_msg_servo_output_raw_send(_channel, + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); + } +}; + + +class MavlinkStreamHILControls : public MavlinkStream +{ +public: + const char *get_name() + { + return "HIL_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHILControls(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + + MavlinkOrbSubscription *act_sub; + struct actuator_outputs_s *act; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); + act = (struct actuator_outputs_s *)act_sub->get_data(); + } + + void send(const hrt_abstime t) + { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + act_sub->update(t); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* set number of valid outputs depending on vehicle type */ + unsigned n; + + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + /* scale / assign outputs depending on system type */ + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + + } else { + out[i] = -1.0f; + } + } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } +}; + + +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ +public: + const char *get_name() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionSetpointInt(); + } + +private: + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + } + + void send(const hrt_abstime t) + { + pos_sp_triplet_sub->update(t); + + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ +public: + const char *get_name() + { + return "LOCAL_POSITION_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionSetpoint(); + } + +private: + MavlinkOrbSubscription *pos_sp_sub; + struct vehicle_local_position_setpoint_s *pos_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); + pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); + } + + void send(const hrt_abstime t) + { + pos_sp_sub->update(t); + + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp->x, + pos_sp->y, + pos_sp->z, + pos_sp->yaw); + } +}; + + +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_sp_sub; + struct vehicle_attitude_setpoint_s *att_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); + att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); + } + + void send(const hrt_abstime t) + { + att_sp_sub->update(t); + + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp->timestamp / 1000, + att_sp->roll_body, + att_sp->pitch_body, + att_sp->yaw_body, + att_sp->thrust); + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_rates_sp_sub; + struct vehicle_rates_setpoint_s *att_rates_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); + att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); + } + + void send(const hrt_abstime t) + { + att_rates_sp_sub->update(t); + + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp->timestamp / 1000, + att_rates_sp->roll, + att_rates_sp->pitch, + att_rates_sp->yaw, + att_rates_sp->thrust); + } +}; + + +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ +public: + const char *get_name() + { + return "RC_CHANNELS_RAW"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRCChannelsRaw(); + } + +private: + MavlinkOrbSubscription *rc_sub; + struct rc_input_values *rc; + +protected: + void subscribe(Mavlink *mavlink) + { + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); + rc = (struct rc_input_values *)rc_sub->get_data(); + } + + void send(const hrt_abstime t) + { + rc_sub->update(t); + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); + } + } +}; + + +class MavlinkStreamManualControl : public MavlinkStream +{ +public: + const char *get_name() + { + return "MANUAL_CONTROL"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamManualControl(); + } + +private: + MavlinkOrbSubscription *manual_sub; + struct manual_control_setpoint_s *manual; + +protected: + void subscribe(Mavlink *mavlink) + { + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); + manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); + } + + void send(const hrt_abstime t) + { + manual_sub->update(t); + + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } +}; + + +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() + { + return "OPTICAL_FLOW"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamOpticalFlow(); + } + +private: + MavlinkOrbSubscription *flow_sub; + struct optical_flow_s *flow; + +protected: + void subscribe(Mavlink *mavlink) + { + flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); + flow = (struct optical_flow_s *)flow_sub->get_data(); + } + + void send(const hrt_abstime t) + { + flow_sub->update(t); + + mavlink_msg_optical_flow_send(_channel, + 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); + } +}; + + +MavlinkStream *streams_list[] = { + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + new MavlinkStreamAttitudeQuaternion(), + new MavlinkStreamVFRHUD(), + new MavlinkStreamGPSRawInt(), + new MavlinkStreamGlobalPositionInt(), + new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), + new MavlinkStreamServoOutputRaw(0), + new MavlinkStreamServoOutputRaw(1), + new MavlinkStreamServoOutputRaw(2), + new MavlinkStreamServoOutputRaw(3), + new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), + new MavlinkStreamManualControl(), + new MavlinkStreamOpticalFlow(), + nullptr +}; + + + + + + + +// +// +// +// +// +// +//void +//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) +//{ +// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl0 ", +// l->listener->actuators_0.control[0]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl1 ", +// l->listener->actuators_0.control[1]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl2 ", +// l->listener->actuators_0.control[2]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl3 ", +// l->listener->actuators_0.control[3]); +// } +//} +// +//void +//MavlinkOrbListener::l_debug_key_value(const struct listener *l) +//{ +// struct debug_key_value_s debug; +// +// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); +// +// /* Enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// debug.key, +// debug.value); +//} +// +//void +//MavlinkOrbListener::l_nav_cap(const struct listener *l) +//{ +// +// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// hrt_absolute_time() / 1000, +// "turn dist", +// l->listener->nav_cap.turn_distance); +// +//} diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_messages.h index 744ed7d94..b8823263a 100644 --- a/src/modules/mavlink/mavlink_hil.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -33,20 +32,17 @@ ****************************************************************************/ /** - * @file mavlink_hil.h - * Hardware-in-the-loop simulation support. + * @file mavlink_messages.h + * MAVLink 1.0 message formatters definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ -#pragma once +#ifndef MAVLINK_MESSAGES_H_ +#define MAVLINK_MESSAGES_H_ -extern bool mavlink_hil_enabled; +#include "mavlink_stream.h" -/** - * 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); +extern MavlinkStream *streams_list[]; + +#endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_orb_subscription.cpp index b72bbb2b1..6279e5366 100644 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2014 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 @@ -33,51 +32,58 @@ ****************************************************************************/ /** - * @file mavlink_bridge_header - * MAVLink bridge header for UART access. + * @file mavlink_orb_subscription.cpp + * uORB subscription implementation. * - * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ -/* 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> +#include <stdlib.h> +#include <string.h> +#include <uORB/uORB.h> +#include <stdio.h> +#include "mavlink_orb_subscription.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 +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) +{ + _data = malloc(topic->o_size); + memset(_data, 0, topic->o_size); + _fd = orb_subscribe(_topic); +} - mavlink_system.sysid = 100; // System ID, 1-255 - mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 +MavlinkOrbSubscription::~MavlinkOrbSubscription() +{ + close(_fd); + free(_data); +} - Lines also in your main.c, e.g. by reading these parameter from EEPROM. - */ -extern mavlink_system_t mavlink_system; +const orb_id_t +MavlinkOrbSubscription::get_topic() +{ + return _topic; +} -/** - * @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); +void * +MavlinkOrbSubscription::get_data() +{ + return _data; +} -mavlink_status_t* mavlink_get_channel_status(uint8_t chan); -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); +bool +MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (_last_check != t) { + _last_check = t; + bool updated; + orb_check(_fd, &updated); -#include <v1.0/common/mavlink.h> + if (updated) { + orb_copy(_topic, _fd, _data); + return true; + } + } -#endif /* MAVLINK_BRIDGE_HEADER_H */ + return false; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h new file mode 100644 index 000000000..eacc27034 --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_orb_subscription.h + * uORB subscription definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef MAVLINK_ORB_SUBSCRIPTION_H_ +#define MAVLINK_ORB_SUBSCRIPTION_H_ + +#include <systemlib/uthash/utlist.h> +#include <drivers/drv_hrt.h> + + +class MavlinkOrbSubscription +{ +public: + MavlinkOrbSubscription *next; + + MavlinkOrbSubscription(const orb_id_t topic); + ~MavlinkOrbSubscription(); + + bool update(const hrt_abstime t); + void *get_data(); + const orb_id_t get_topic(); + +private: + const orb_id_t _topic; + int _fd; + void *_data; + hrt_abstime _last_check; +}; + + +#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */ diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c deleted file mode 100644 index 18ca7a854..000000000 --- a/src/modules/mavlink/mavlink_parameters.c +++ /dev/null @@ -1,230 +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 "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/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h deleted file mode 100644 index b1e38bcc8..000000000 --- a/src/modules/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/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp new file mode 100644 index 000000000..9cdda8b17 --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_rate_limiter.cpp + * Message rate limiter implementation. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "mavlink_rate_limiter.h" + +MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000) +{ +} + +MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval) +{ +} + +MavlinkRateLimiter::~MavlinkRateLimiter() +{ +} + +void +MavlinkRateLimiter::set_interval(unsigned int interval) +{ + _interval = interval; +} + +bool +MavlinkRateLimiter::check(hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + + if (dt > 0 && dt >= _interval) { + _last_sent = (t / _interval) * _interval; + return true; + } + + return false; +} diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink/mavlink_rate_limiter.h index c84b6fd26..0b37538e6 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2014 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 @@ -33,23 +32,31 @@ ****************************************************************************/ /** - * @file util.h - * Utility and helper functions and data. + * @file mavlink_rate_limiter.h + * Message rate limiter definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ -#pragma once +#ifndef MAVLINK_RATE_LIMITER_H_ +#define MAVLINK_RATE_LIMITER_H_ -#include "orb_topics.h" +#include <drivers/drv_hrt.h> -/** MAVLink communications channel */ -extern uint8_t chan; -/** Shutdown marker */ -extern volatile bool thread_should_exit; +class MavlinkRateLimiter +{ +private: + hrt_abstime _last_sent; + hrt_abstime _interval; -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, - uint8_t *mavlink_state, uint8_t *mavlink_mode); +public: + MavlinkRateLimiter(); + MavlinkRateLimiter(unsigned int interval); + ~MavlinkRateLimiter(); + void set_interval(unsigned int interval); + bool check(hrt_abstime t); +}; + + +#endif /* MAVLINK_RATE_LIMITER_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1dbe56495..ef1a747da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -33,10 +32,11 @@ ****************************************************************************/ /** - * @file mavlink_receiver.c + * @file mavlink_receiver.cpp * MAVLink protocol message receive and dispatch * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ /* XXX trim includes */ @@ -77,731 +77,787 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "mavlink_parameters.h" +#include "mavlink_receiver.h" +#include "mavlink_main.h" #include "util.h" -extern bool gcs_link; - __END_DECLS -/* XXX should be in a header somewhere */ -extern "C" 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_local_position_s hil_local_pos; -struct vehicle_attitude_s hil_attitude; -struct vehicle_gps_position_s hil_gps; -struct sensor_combined_s hil_sensors; -struct battery_status_s hil_battery_status; -static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t pub_hil_local_pos = -1; -static orb_advert_t pub_hil_attitude = -1; -static orb_advert_t pub_hil_gps = -1; -static orb_advert_t pub_hil_sensors = -1; -static orb_advert_t pub_hil_gyro = -1; -static orb_advert_t pub_hil_accel = -1; -static orb_advert_t pub_hil_mag = -1; -static orb_advert_t pub_hil_baro = -1; -static orb_advert_t pub_hil_airspeed = -1; -static orb_advert_t pub_hil_battery = -1; - -/* packet counter */ -static int hil_counter = 0; -static int hil_frames = 0; -static uint64_t old_timestamp = 0; - -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; - -static orb_advert_t offboard_control_sp_pub = -1; -static orb_advert_t vicon_position_pub = -1; -static orb_advert_t telemetry_status_pub = -1; - -// variables for HIL reference position -static int32_t lat0 = 0; -static int32_t lon0 = 0; -static double alt0 = 0; - -static void -handle_message(mavlink_message_t *msg) +static const float mg2ms2 = 9.8f / 1000.0f; + +MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : + _mavlink(parent), + + _manual_sub(-1), + + _global_pos_pub(-1), + _local_pos_pub(-1), + _attitude_pub(-1), + _gps_pub(-1), + _sensors_pub(-1), + _gyro_pub(-1), + _accel_pub(-1), + _mag_pub(-1), + _baro_pub(-1), + _airspeed_pub(-1), + _battery_pub(-1), + _cmd_pub(-1), + _flow_pub(-1), + _offboard_control_sp_pub(-1), + _vicon_position_pub(-1), + _telemetry_status_pub(-1), + _rc_pub(-1), + _manual_pub(-1), + + _hil_counter(0), + _hil_frames(0), + _old_timestamp(0), + _hil_local_proj_inited(0), + _hil_local_alt0(0.0) { - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + memset(&hil_local_pos, 0, sizeof(hil_local_pos)); +} - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); +void +MavlinkReceiver::handle_message(mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: + handle_message_command_long(msg); + break; - 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); + case MAVLINK_MSG_ID_OPTICAL_FLOW: + handle_message_optical_flow(msg); + break; - /* terminate other threads and this thread */ - thread_should_exit = true; + case MAVLINK_MSG_ID_SET_MODE: + handle_message_set_mode(msg); + break; - } else { + case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: + handle_message_vicon_position_estimate(msg); + break; - /* 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; - // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)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); - - } else { - /* 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); + case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: + handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); + break; - struct optical_flow_s f; + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status(msg); + break; - 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; + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; - /* 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); - } + default: + break; } - 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); - - union px4_custom_mode custom_mode; - custom_mode.data = new_mode.custom_mode; - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.main_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = VEHICLE_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); + /* + * 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->get_hil_enabled()) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; + + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; + + default: + break; } } +} - /* 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); +void +MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; } else { - orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* 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; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)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); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } } } +} - /* 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) { +void +MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub <= 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } +} - /* switch to a receiving link mode */ - gcs_link = false; +void +MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) +{ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; + /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = custom_mode.main_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = VEHICLE_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; + + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } +} - /* - * rate control mode - defined by MAVLink - */ +void +MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) +{ + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); - uint8_t ml_mode = 0; - bool ml_armed = false; + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; + 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; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; + if (_vicon_position_pub <= 0) { + _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - break; + } else { + orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); + } +} - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; +void +MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) +{ + 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); - break; + if (mavlink_system.sysid < 4) { + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; + uint8_t ml_mode = 0; + bool ml_armed = false; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + 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; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } + break; - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; - offboard_control_sp.timestamp = hrt_absolute_time(); + break; - /* 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); + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; - } else { - /* Publish */ - orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); - } + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; } - } - /* handle status updates of the radio */ - if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + 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; - struct telemetry_status_s tstatus; + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + ml_armed = false; + } - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); - /* publish telemetry status topic */ - tstatus.timestamp = hrt_absolute_time(); - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; + offboard_control_sp.timestamp = hrt_absolute_time(); - if (telemetry_status_pub <= 0) { - telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_offboard_control_sp_pub <= 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { - orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus); + 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(); - - if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); - - /* sensors general */ - hil_sensors.timestamp = hrt_absolute_time(); - - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - 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; - hil_sensors.gyro_counter = hil_counter; - - /* accelerometer */ - 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 - hil_sensors.accelerometer_counter = hil_counter; - - /* adc */ - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - - /* magnetometer */ - float mga2ga = 1.0e-3f; - 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.magnetometer_counter = hil_counter; - - /* baro */ - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = hil_counter; - - /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; - - /* airspeed from differential pressure, ambient pressure and temp */ - struct airspeed_s airspeed; - - float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); - // XXX need to fix this - float tas = ias; - - airspeed.timestamp = hrt_absolute_time(); - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; - - if (pub_hil_airspeed < 0) { - pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); - } - - //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - - /* individual sensor publications */ - struct gyro_report gyro; - gyro.x_raw = imu.xgyro / mrad2rad; - gyro.y_raw = imu.ygyro / mrad2rad; - gyro.z_raw = imu.zgyro / mrad2rad; - gyro.x = imu.xgyro; - gyro.y = imu.ygyro; - gyro.z = imu.zgyro; - gyro.temperature = imu.temperature; - gyro.timestamp = hrt_absolute_time(); - - if (pub_hil_gyro < 0) { - pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); - - } else { - orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); - } - - struct accel_report accel; +void +MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) +{ + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub <= 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } +} - accel.x_raw = imu.xacc / mg2ms2; +void +MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) +{ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); - accel.y_raw = imu.yacc / mg2ms2; + /* rc channels */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); - accel.z_raw = imu.zacc / mg2ms2; + rc.timestamp = hrt_absolute_time(); + rc.chan_count = 4; - accel.x = imu.xacc; + rc.chan[0].scaled = man.x / 1000.0f; + rc.chan[1].scaled = man.y / 1000.0f; + rc.chan[2].scaled = man.r / 1000.0f; + rc.chan[3].scaled = man.z / 1000.0f; - accel.y = imu.yacc; + if (_rc_pub == 0) { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - accel.z = imu.zacc; + } else { + orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); + } + } - accel.temperature = imu.temperature; + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - accel.timestamp = hrt_absolute_time(); + /* 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, &manual); - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + manual.timestamp = hrt_absolute_time(); + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); - } + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - struct mag_report mag; + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } + } +} - mag.x_raw = imu.xmag / mga2ga; +void +MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); - mag.y_raw = imu.ymag / mga2ga; + uint64_t timestamp = hrt_absolute_time(); - mag.z_raw = imu.zmag / mga2ga; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - mag.x = imu.xmag; + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; - mag.y = imu.ymag; + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; - mag.z = imu.zmag; + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - mag.timestamp = hrt_absolute_time(); + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } - if (pub_hil_mag < 0) { - pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); - } else { - orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); - } + gyro.timestamp = timestamp; + gyro.x_raw = imu.xgyro * 1000.0f; + gyro.y_raw = imu.ygyro * 1000.0f; + gyro.z_raw = imu.zgyro * 1000.0f; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; - struct baro_report baro; + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - baro.pressure = imu.abs_pressure; + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } - baro.altitude = imu.pressure_alt; + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - baro.temperature = imu.temperature; + accel.timestamp = timestamp; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; - baro.timestamp = hrt_absolute_time(); + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - if (pub_hil_baro < 0) { - pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } - } else { - orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); - } + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); - /* publish combined sensor topic */ - if (pub_hil_sensors > 0) { - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + mag.timestamp = timestamp; + mag.x_raw = imu.xmag * 1000.0f; + mag.y_raw = imu.ymag * 1000.0f; + mag.z_raw = imu.zmag * 1000.0f; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; - } else { - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - } + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - /* fill in HIL battery status */ - hil_battery_status.timestamp = hrt_absolute_time(); - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } - /* lazily publish the battery voltage */ - if (pub_hil_battery > 0) { - orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); - } else { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; - // increment counters - hil_counter++; - hil_frames++; + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames / 10); - old_timestamp = timestamp; - hil_frames = 0; - } + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); } + } - if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - - mavlink_hil_gps_t gps; - mavlink_msg_hil_gps_decode(msg, &gps); - - /* 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.timestamp_variance = gps.time_usec; - 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.timestamp_velocity = gps.time_usec; - hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m - hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m - hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m - hil_gps.vel_ned_valid = true; - /* COG (course over ground) is spec'ed as -PI..+PI */ - hil_gps.cog_rad = heading_rad; - hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; - - /* publish GPS measurement data */ - if (pub_hil_gps > 0) { - orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); - - } else { - pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - } + /* sensor combined */ + { + struct sensor_combined_s hil_sensors; + memset(&hil_sensors, 0, sizeof(hil_sensors)); + + hil_sensors.timestamp = timestamp; + + hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; + hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; + hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = _hil_counter; + + 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 + hil_sensors.accelerometer_counter = _hil_counter; + + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; + + hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; + hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; + hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; + 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.magnetometer_counter = _hil_counter; + + hil_sensors.baro_pres_mbar = imu.abs_pressure; + hil_sensors.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.baro_counter = _hil_counter; + + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_counter = _hil_counter; + + /* publish combined sensor topic */ + if (_sensors_pub > 0) { + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + } else { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } + } - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - - mavlink_hil_state_quaternion_t hil_state; - mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - - struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); - airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; - airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - if (pub_hil_airspeed < 0) { - pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - } else { - orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); - } + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - uint64_t timestamp = hrt_absolute_time(); - - // publish global position - if (pub_hil_global_pos > 0) { - orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); - // global position packet - hil_global_pos.timestamp = timestamp; - hil_global_pos.global_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.vel_n = hil_state.vx / 100.0f; - hil_global_pos.vel_e = hil_state.vy / 100.0f; - hil_global_pos.vel_d = hil_state.vz / 100.0f; + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + } - } else { - pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - } + /* increment counters */ + _hil_counter++; + _hil_frames++; - // publish local position - if (pub_hil_local_pos > 0) { - float x; - float y; - bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? - double lat = hil_state.lat*1e-7; - double lon = hil_state.lon*1e-7; - map_projection_project(lat, lon, &x, &y); - hil_local_pos.timestamp = timestamp; - hil_local_pos.xy_valid = true; - hil_local_pos.z_valid = true; - hil_local_pos.v_xy_valid = true; - hil_local_pos.v_z_valid = true; - hil_local_pos.x = x; - hil_local_pos.y = y; - hil_local_pos.z = alt0 - hil_state.alt/1000.0f; - hil_local_pos.vx = hil_state.vx/100.0f; - hil_local_pos.vy = hil_state.vy/100.0f; - hil_local_pos.vz = hil_state.vz/100.0f; - hil_local_pos.yaw = hil_attitude.yaw; - hil_local_pos.xy_global = true; - hil_local_pos.z_global = true; - hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; - hil_local_pos.ref_alt = alt0; - hil_local_pos.landed = landed; - orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); - } else { - pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); - lat0 = hil_state.lat; - lon0 = hil_state.lon; - alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); - } + /* print HIL sensors rate */ + if ((timestamp - _old_timestamp) > 10000000) { + printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + _old_timestamp = timestamp; + _hil_frames = 0; + } +} - /* Calculate Rotation Matrix */ - math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb = q.to_dcm(); - math::Vector<3> euler = C_nb.to_euler(); +void +MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) +{ + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - hil_attitude.R[i][j] = C_nb(i, j); + uint64_t timestamp = hrt_absolute_time(); - hil_attitude.R_valid = true; + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); - /* set quaternion */ - hil_attitude.q[0] = q(0); - hil_attitude.q[1] = q(1); - hil_attitude.q[2] = q(2); - hil_attitude.q[3] = q(3); - hil_attitude.q_valid = true; + hil_gps.timestamp_time = timestamp; + hil_gps.time_gps_usec = gps.time_usec; - hil_attitude.roll = euler(0); - hil_attitude.pitch = euler(1); - hil_attitude.yaw = euler(2); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; + hil_gps.timestamp_position = timestamp; + 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 - /* set timestamp and notify processes (broadcast) */ - hil_attitude.timestamp = hrt_absolute_time(); + hil_gps.timestamp_variance = timestamp; + hil_gps.s_variance_m_s = 5.0f; + hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; - if (pub_hil_attitude > 0) { - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + hil_gps.timestamp_velocity = timestamp; + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m + hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m + hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m + hil_gps.vel_ned_valid = true; + hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - } else { - pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - } + hil_gps.timestamp_satellites = timestamp; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; - struct accel_report accel; + if (_gps_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); - accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + } else { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } +} - accel.y_raw = hil_state.yacc / 9.81f * 1e3f; +void +MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) +{ + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + uint64_t timestamp = hrt_absolute_time(); - accel.x = hil_state.xacc; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - accel.y = hil_state.yacc; + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - accel.z = hil_state.zacc; + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - accel.temperature = 25.0f; + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } - accel.timestamp = hrt_absolute_time(); + /* attitude */ + struct vehicle_attitude_s hil_attitude; + { + memset(&hil_attitude, 0, sizeof(hil_attitude)); + math::Quaternion q(hil_state.attitude_quaternion); + math::Matrix<3, 3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); + + hil_attitude.timestamp = timestamp; + memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); + hil_attitude.R_valid = true; + + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + if (_attitude_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); - } + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - /* fill in HIL battery status */ - hil_battery_status.timestamp = hrt_absolute_time(); - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; + hil_global_pos.timestamp = timestamp; + hil_global_pos.global_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.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; + hil_global_pos.yaw = hil_attitude.yaw; - /* lazily publish the battery voltage */ - if (pub_hil_battery > 0) { - orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + if (_global_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); - } else { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } + } else { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } + } - 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; + /* local position */ + { + if (!_hil_local_proj_inited) { + _hil_local_proj_inited = true; + _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = _hil_local_alt0; + } - struct manual_control_setpoint_s mc; - static orb_advert_t mc_pub = 0; + float x; + float y; + map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + hil_local_pos.landed = landed; + + if (_local_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + } else { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + } + } - /* 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); + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - 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; + accel.timestamp = timestamp; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; - /* fake RC channels with manual control input from simulator */ + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } - if (rc_pub == 0) { - rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - } else { - orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); - } + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - if (mc_pub == 0) { - mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } else { - orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); - } + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } } @@ -810,17 +866,22 @@ handle_message(mavlink_message_t *msg) /** * Receive data from UART. */ -static void * -receive_thread(void *arg) +void * +MavlinkReceiver::receive_thread(void *arg) { - int uart_fd = *((int *)arg); + int uart_fd = _mavlink->get_uart_fd(); - const int timeout = 1000; + const int timeout = 500; uint8_t buf[32]; mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); + /* set thread name */ + char thread_name[18]; + sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); + + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct pollfd fds[1]; fds[0].fd = uart_fd; @@ -828,27 +889,26 @@ receive_thread(void *arg) ssize_t nread = 0; - while (!thread_should_exit) { + while (!_mavlink->_task_should_exit) { if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { + + /* non-blocking read. read may return negative values */ + if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } - /* non-blocking read. read may return negative values */ - nread = read(uart_fd, buf, sizeof(buf)); - /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(chan, buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); /* handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg); + _mavlink->mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } } @@ -857,15 +917,26 @@ receive_thread(void *arg) return NULL; } +void MavlinkReceiver::print_status() +{ + +} + +void *MavlinkReceiver::start_helper(void *context) +{ + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); + return rcv->receive_thread(NULL); +} + pthread_t -receive_start(int uart) +MavlinkReceiver::receive_start(Mavlink *parent) { 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); + int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); + fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -875,7 +946,7 @@ receive_start(int uart) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/mavlink_receiver.h index 89f647bdc..beaae2058 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-2014 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 @@ -33,12 +32,15 @@ ****************************************************************************/ /** - * @file orb_topics.h - * Common sets of topics subscribed to or published by the MAVLink driver, - * and structures maintained by those subscriptions. + * @file mavlink_orb_listener.h + * MAVLink 1.0 uORB listener definition + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ + #pragma once +#include <systemlib/perf_counter.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> @@ -55,7 +57,6 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -66,59 +67,81 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> -#include <drivers/drv_rc_input.h> -#include <uORB/topics/navigation_capabilities.h> -struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int safety_sub; - int actuators_sub; - int armed_sub; - int actuators_effective_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int triplet_sub; - int debug_key_value; - int input_rc_sub; - int optical_flow; - int rates_setpoint_sub; - int home_sub; - int airspeed_sub; - int navigation_capabilities_sub; - int position_setpoint_triplet_sub; -}; +class Mavlink; -extern struct mavlink_subscriptions mavlink_subs; +class MavlinkReceiver +{ +public: + /** + * Constructor + */ + MavlinkReceiver(Mavlink *parent); -/** Global position */ -extern struct vehicle_global_position_s global_pos; + /** + * Destructor, also kills the mavlinks task. + */ + ~MavlinkReceiver(); -/** Local position */ -extern struct vehicle_local_position_s local_pos; + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); -/** navigation capabilities */ -extern struct navigation_capabilities_s nav_cap; + /** + * Display the mavlink status. + */ + void print_status(); -/** Vehicle status */ -extern struct vehicle_status_s v_status; + static pthread_t receive_start(Mavlink *parent); -/** Position setpoint triplet */ -extern struct position_setpoint_triplet_s pos_sp_triplet; + static void *start_helper(void *context); -/** RC channels */ -extern struct rc_channels_s rc; +private: + perf_counter_t _loop_perf; /**< loop performance counter */ -/** Actuator armed state */ -extern struct actuator_armed_s armed; + Mavlink *_mavlink; -/** Worker thread starter */ -extern pthread_t uorb_receive_start(void); + void handle_message(mavlink_message_t *msg); + void handle_message_command_long(mavlink_message_t *msg); + void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_set_mode(mavlink_message_t *msg); + void handle_message_vicon_position_estimate(mavlink_message_t *msg); + void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_radio_status(mavlink_message_t *msg); + void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_hil_sensor(mavlink_message_t *msg); + void handle_message_hil_gps(mavlink_message_t *msg); + void handle_message_hil_state_quaternion(mavlink_message_t *msg); + + void *receive_thread(void *arg); + + mavlink_status_t status; + struct vehicle_local_position_s hil_local_pos; + int _manual_sub; + orb_advert_t _global_pos_pub; + orb_advert_t _local_pos_pub; + orb_advert_t _attitude_pub; + orb_advert_t _gps_pub; + orb_advert_t _sensors_pub; + orb_advert_t _gyro_pub; + orb_advert_t _accel_pub; + orb_advert_t _mag_pub; + orb_advert_t _baro_pub; + orb_advert_t _airspeed_pub; + orb_advert_t _battery_pub; + orb_advert_t _cmd_pub; + orb_advert_t _flow_pub; + orb_advert_t _offboard_control_sp_pub; + orb_advert_t _vicon_position_pub; + orb_advert_t _telemetry_status_pub; + orb_advert_t _rc_pub; + orb_advert_t _manual_pub; + int _hil_counter; + int _hil_frames; + uint64_t _old_timestamp; + bool _hil_local_proj_inited; + float _hil_local_alt0; +}; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp new file mode 100644 index 000000000..bb19d7e33 --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_stream.cpp + * Mavlink messages stream implementation. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdlib.h> + +#include "mavlink_stream.h" +#include "mavlink_main.h" + +MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +{ +} + +MavlinkStream::~MavlinkStream() +{ +} + +/** + * Set messages interval in ms + */ +void +MavlinkStream::set_interval(const unsigned int interval) +{ + _interval = interval; +} + +/** + * Set mavlink channel + */ +void +MavlinkStream::set_channel(mavlink_channel_t channel) +{ + _channel = channel; +} + +/** + * Update subscriptions and send message if necessary + */ +int +MavlinkStream::update(const hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + + if (dt > 0 && dt >= _interval) { + /* interval expired, send message */ + send(t); + _last_sent = (t / _interval) * _interval; + } +} diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h new file mode 100644 index 000000000..135e1bce0 --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +/** + * @file mavlink_stream.cpp + * Mavlink messages stream definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef MAVLINK_STREAM_H_ +#define MAVLINK_STREAM_H_ + +#include <drivers/drv_hrt.h> + +class Mavlink; +class MavlinkStream; + +#include "mavlink_main.h" + +class MavlinkStream +{ +private: + hrt_abstime _last_sent; + +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + +public: + MavlinkStream *next; + + MavlinkStream(); + ~MavlinkStream(); + void set_interval(const unsigned int interval); + void set_channel(mavlink_channel_t channel); + int update(const hrt_abstime t); + virtual MavlinkStream *new_instance() = 0; + virtual void subscribe(Mavlink *mavlink) = 0; + virtual const char *get_name() = 0; +}; + + +#endif /* MAVLINK_STREAM_H_ */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 89a097c24..f09efa2e6 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 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 @@ -36,10 +36,12 @@ # MODULE_COMMAND = mavlink -SRCS += mavlink.c \ - mavlink_parameters.c \ - mavlink_receiver.cpp \ - orb_listener.c \ - waypoints.c +SRCS += mavlink_main.cpp \ + mavlink.c \ + mavlink_receiver.cpp \ + mavlink_orb_subscription.cpp \ + mavlink_messages.cpp \ + mavlink_stream.cpp \ + mavlink_rate_limiter.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c deleted file mode 100644 index d7243c623..000000000 --- a/src/modules/mavlink/orb_listener.c +++ /dev/null @@ -1,848 +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 <drivers/drv_hrt.h> -#include <time.h> -#include <float.h> -#include <unistd.h> -#include <sys/prctl.h> -#include <stdlib.h> -#include <poll.h> -#include <lib/geo/geo.h> - -#include <mavlink/mavlink_log.h> - -#include "waypoints.h" -#include "orb_topics.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 home_position_s home; -struct navigation_capabilities_s nav_cap; -struct vehicle_status_s v_status; -struct position_setpoint_triplet_s pos_sp_triplet; -struct rc_channels_s rc; -struct rc_input_values rc_raw; -struct actuator_armed_s armed; -struct actuator_controls_s actuators_0; -struct vehicle_attitude_s att; -struct airspeed_s airspeed; - -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 hrt_abstime last_sent_vfr = 0; - -static void *uorb_receive_thread(void *arg); - -struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; -}; - -uint16_t cm_uint16_from_m_float(float m); - -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 void l_airspeed(const struct listener *l); -static void l_nav_cap(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.triplet_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}, - {l_airspeed, &mavlink_subs.airspeed_sub, 0}, - {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, -}; - -static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); - -uint16_t -cm_uint16_from_m_float(float m) -{ - if (m < 0.0f) { - return 0; - - } else if (m > 655.35f) { - return 65535; - } - - return (uint16_t)(m * 100.0f); -} - -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, raw.differential_pressure_pa, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); - - sensors_raw_counter++; -} - -void -l_vehicle_attitude(const struct listener *l) -{ - /* 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); - - /* limit VFR message rate to 10Hz */ - hrt_abstime t = hrt_absolute_time(); - if (t >= last_sent_vfr + 100000) { - last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); - } - - /* send quaternion values if it exists */ - if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - 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 = _wrap_2pi(gps.cog_rad) * 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, - cm_uint16_from_m_float(gps.eph_m), - cm_uint16_from_m_float(gps.epv_m), - gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from deg 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); - orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet); - - /* enable or disable HIL */ - if (v_status.hil_state == HIL_STATE_ON) - set_hil_on_off(true); - else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - 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) { - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp_publication / 1000, - i, - (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, - rc_raw.rssi); - } - } -} - -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); - - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, - global_pos.timestamp / 1000, - global_pos.lat * 1e7, - global_pos.lon * 1e7, - global_pos.alt * 1000.0f, - (global_pos.alt - home.alt) * 1000.0f, - global_pos.vel_n * 100.0f, - global_pos.vel_e * 100.0f, - global_pos.vel_d * 100.0f, - _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -} - -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 position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - - if (!triplet.current.valid) - return; - - if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - MAV_FRAME_GLOBAL, - (int32_t)(triplet.current.lat * 1e7d), - (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.alt * 1e3f), - (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -} - -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 - needs GCS support */, - /* QGC has port number support already */ - 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 and only send first group for HIL */ - if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_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_base_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_base_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_base_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_base_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) -{ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); - - 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, - "ctrl0 ", - actuators_0.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl1 ", - actuators_0.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl2 ", - actuators_0.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl3 ", - actuators_0.control[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) -{ - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); -} - -void -l_airspeed(const struct listener *l) -{ - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); -} - -void -l_nav_cap(const struct listener *l) -{ - - orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); - - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - hrt_absolute_time() / 1000, - "turn dist", - nav_cap.turn_distance); - -} - -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) { - /* silent */ - - } 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 */ - - /* --- POSITION SETPOINT TRIPLET --- */ - mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */ - - /* --- 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.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink_subs.triplet_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); - 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 */ - - /* --- AIRSPEED --- */ - mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ - - /* --- NAVIGATION CAPABILITIES --- */ - mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ - nav_cap.turn_distance = 0.0f; - - /* 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); - - pthread_attr_destroy(&uorb_attr); - return thread; -} diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index 5e5ee8261..5ca9a085d 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-2014 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 diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c deleted file mode 100644 index 168666d4e..000000000 --- a/src/modules/mavlink/waypoints.c +++ /dev/null @@ -1,730 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 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 "mavlink_bridge_header.h" -#include "waypoints.h" -#include "util.h" -#include <uORB/uORB.h> -#include <uORB/topics/mission.h> -#include <geo/geo.h> -#include <dataman/dataman.h> -#include <drivers/drv_hrt.h> -#include <systemlib/err.h> - -bool verbose = true; - -orb_advert_t mission_pub = -1; -struct mission_s mission; - -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - -void -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); -} - - - -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); - mavlink_missionlib_send_message(&msg); - return OK; - - } else { - return 1; - } -} - -void publish_mission() -{ - /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); - - } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); - } -} - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = mavlink_mission_item->command; - - mission_item->time_inside = mavlink_mission_item->param1; - mission_item->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - return OK; -} - -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; - break; - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; - mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside; - mavlink_mission_item->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void mavlink_wpm_init(mavlink_wpm_storage *state) -{ - 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->current_dataman_id = 0; -} - -/* - * @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 = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); -} - -/* - * @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_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); - - } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (verbose) warnx("ERROR: index out of bounds"); - } -} - -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 = sysid; - wpc.target_component = compid; - wpc.count = mission.count; - - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - - struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_current; - - if (wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - if (dm_read(dm_current, seq, &mission_item, len) == len) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); - } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: could not read WP%u", seq); - } -} - -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 = sysid; - wpr.target_component = compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); - } -} - -/* - * @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 (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); -} - - -void mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_partner_sysid = 0; - wpm->current_partner_compid = 0; - } -} - - -void mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - 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) { - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); - } - - 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) { - - mission.current_index = wpc.seq; - publish_mission(); - - mavlink_wpm_send_waypoint_current(wpc.seq); - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (verbose) warnx("IGN WP CURR CMD: Not in list"); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - if (verbose) warnx("IGN WP CURR CMD: Busy"); - - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("REJ. WP CMD: target id mismatch"); - } - - 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) { - - 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) warnx("No waypoints send"); - } - - wpm->current_count = wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); - - } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (verbose) warnx("IGN REQUEST LIST: Busy"); - } - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); - } - - 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; - - if (wpr.seq >= wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); - break; - } - - /* - * 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) { - - if (wpr.seq == 0) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (verbose) warnx("REJ. WP CMD: First id != 0"); - break; - } - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - - if (wpr.seq == wpm->current_wp_id) { - - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - - } else if (wpr.seq == wpm->current_wp_id + 1) { - - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); - break; - } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); - break; - } - - wpm->current_wp_id = wpr.seq; - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < wpm->size) { - - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); - - } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); - } - - - } 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)) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - } - 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) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); - break; - } - - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); - - 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; - - 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_GETLIST) { - - if (wpm->current_wp_id == 0) { - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); - } - } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); - } - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { - - wpm->timestamp_lastaction = now; - - /* - * 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) { - - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); - break; - } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (wp.seq >= wpm->current_count) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); - break; - } - - if (wp.seq != wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - break; - } - } - - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_next; - - if (wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - if (wp.current) { - mission.current_index = wp.seq; - } - - wpm->current_wp_id = wp.seq + 1; - - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - mission.count = wpm->current_count; - - publish_mission(); - - wpm->current_dataman_id = mission.dataman_id; - wpm->size = wpm->current_count; - - 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 { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - - 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 */) { - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; - - wpm->size = 0; - - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); - - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (verbose) warnx("IGN WP CLEAR CMD: Busy"); - } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index f8b58c7d9..532eff7aa 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * 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> + * Copyright (c) 2009-2014 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 @@ -37,6 +34,11 @@ /** * @file waypoints.h * MAVLink waypoint protocol definition (BSD-relicensed). + * + * @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> */ #ifndef WAYPOINTS_H_ @@ -106,7 +108,4 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; -void mavlink_missionlib_send_message(mavlink_message_t *msg); -int mavlink_missionlib_send_gcs_string(const char *string); - #endif /* WAYPOINTS_H_ */ diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c deleted file mode 100644 index ab9ce45f3..000000000 --- a/src/modules/mavlink_onboard/mavlink.c +++ /dev/null @@ -1,537 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink.c - * MAVLink 1.0 protocol implementation. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <pthread.h> -#include <stdio.h> -#include <math.h> -#include <stdbool.h> -#include <fcntl.h> -#include <mqueue.h> -#include <string.h> -#include "mavlink_bridge_header.h" -#include <drivers/drv_hrt.h> -#include <time.h> -#include <float.h> -#include <unistd.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <stdlib.h> -#include <poll.h> - -#include <systemlib/param/param.h> -#include <systemlib/systemlib.h> -#include <systemlib/err.h> - -#include "orb_topics.h" -#include "util.h" - -__EXPORT int mavlink_onboard_main(int argc, char *argv[]); - -static int mavlink_thread_main(int argc, char *argv[]); - -/* thread state */ -volatile bool thread_should_exit = false; -static volatile bool thread_running = false; -static int mavlink_task; - -/* pthreads */ -static pthread_t receive_thread; - -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; - -mavlink_system_t mavlink_system = { - 100, - 50, - MAV_TYPE_QUADROTOR, - 0, - 0, - 0 -}; // System ID, 1-255, Component/Subsystem ID, 1-255 - -/* XXX not widely used */ -uint8_t chan = MAVLINK_COMM_0; - -/* XXX probably should be in a header... */ -extern pthread_t receive_start(int uart); - -bool mavlink_hil_enabled = false; - -/* protocol interface */ -static int uart; -static int baudrate; -bool gcs_link = true; - -/* interface mode */ -static enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD -} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; - -static void mavlink_update_system(void); -static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -static void usage(void); - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 0: speed = B0; break; - - case 50: speed = B50; break; - - case 75: speed = B75; break; - - case 110: speed = B110; break; - - case 134: speed = B134; break; - - case 150: speed = B150; break; - - case 200: speed = B200; break; - - case 300: speed = B300; break; - - case 600: speed = B600; break; - - case 1200: speed = B1200; break; - - case 1800: speed = B1800; break; - - case 2400: speed = B2400; break; - - case 4800: speed = B4800; break; - - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - - case 460800: speed = B460800; break; - - case 921600: speed = B921600; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud); - return -EINVAL; - } - - /* open uart */ - warnx("UART is %s, baudrate is %d", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - *is_usb = false; - - if (strcmp(uart_name, "/dev/ttyACM0") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state); - close(uart); - return -1; - } - - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; - } - - return uart; -} - -void -mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) -{ - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); -} - -/* - * Internal function to give access to the channel status for each channel - */ -mavlink_status_t* mavlink_get_channel_status(uint8_t channel) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[channel]; -} - -/* - * Internal function to give access to the channel buffer for each channel - */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) -{ - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[channel]; -} - -void mavlink_update_system(void) -{ - static bool initialized = false; - param_t param_system_id; - param_t param_component_id; - param_t param_system_type; - - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - } - - /* update system and component id */ - int32_t system_id; - param_get(param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - int32_t component_id; - param_get(param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - int32_t system_type; - param_get(param_system_type, &system_type); - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; - } -} - -void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, - uint8_t *mavlink_state, uint8_t *mavlink_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; - - /* set mode flags independent of system state */ - if (control_mode->flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - if (control_mode->flag_system_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* set arming state */ - if (armed->armed) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - } - - if (control_mode->flag_control_velocity_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; - } - -// switch (v_status->state_machine) { -// case SYSTEM_STATE_PREFLIGHT: -// if (v_status->flag_preflight_gyro_calibration || -// v_status->flag_preflight_mag_calibration || -// v_status->flag_preflight_accel_calibration) { -// *mavlink_state = MAV_STATE_CALIBRATING; -// } else { -// *mavlink_state = MAV_STATE_UNINIT; -// } -// break; -// -// case SYSTEM_STATE_STANDBY: -// *mavlink_state = MAV_STATE_STANDBY; -// break; -// -// case SYSTEM_STATE_GROUND_READY: -// *mavlink_state = MAV_STATE_ACTIVE; -// break; -// -// case SYSTEM_STATE_MANUAL: -// *mavlink_state = MAV_STATE_ACTIVE; -// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; -// break; -// -// case SYSTEM_STATE_STABILIZED: -// *mavlink_state = MAV_STATE_ACTIVE; -// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; -// break; -// -// case SYSTEM_STATE_AUTO: -// *mavlink_state = MAV_STATE_ACTIVE; -// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; -// break; -// -// case SYSTEM_STATE_MISSION_ABORT: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_EMCY_LANDING: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_EMCY_CUTOFF: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_GROUND_ERROR: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_REBOOT: -// *mavlink_state = MAV_STATE_POWEROFF; -// break; -// } - -} - -/** - * MAVLink Protocol main function. - */ -int mavlink_thread_main(int argc, char *argv[]) -{ - int ch; - char *device_name = "/dev/ttyS1"; - baudrate = 57600; - - /* XXX this is never written? */ - struct vehicle_status_s v_status; - struct vehicle_control_mode_s control_mode; - struct actuator_armed_s armed; - - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { - switch (ch) { - case 'b': - baudrate = strtoul(optarg, NULL, 10); - if (baudrate == 0) - errx(1, "invalid baud rate '%s'", optarg); - break; - - case 'd': - device_name = optarg; - break; - - case 'e': - mavlink_link_termination_allowed = true; - break; - - case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; - break; - - default: - usage(); - } - } - - struct termios uart_config_original; - bool usb_uart; - - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - - /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); - - /* Flush stdout in case MAVLink is about to take it over */ - fflush(stdout); - - /* default values for arguments */ - uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) - err(1, "could not open %s", device_name); - - /* Initialize system properties */ - mavlink_update_system(); - - /* start the MAVLink receiver */ - receive_thread = receive_start(uart); - - thread_running = true; - - /* arm counter to go off immediately */ - unsigned lowspeed_counter = 10; - - while (!thread_should_exit) { - - /* 1 Hz */ - if (lowspeed_counter == 10) { - mavlink_update_system(); - - /* translate the current system state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); - - /* send heartbeat */ - // TODO fix navigation state, use control_mode topic - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state); - - /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, - v_status.onboard_control_sensors_present, - v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, - v_status.load * 1000.0f, - v_status.battery_voltage * 1000.0f, - v_status.battery_current * 1000.0f, - v_status.battery_remaining, - v_status.drop_rate_comm, - v_status.errors_comm, - v_status.errors_count1, - v_status.errors_count2, - v_status.errors_count3, - v_status.errors_count4); - lowspeed_counter = 0; - } - lowspeed_counter++; - - /* sleep 1000 ms */ - usleep(1000000); - } - - /* wait for threads to complete */ - pthread_join(receive_thread, NULL); - - /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); - - thread_running = false; - - exit(0); -} - -static void -usage() -{ - fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n" - " mavlink_onboard stop\n" - " mavlink_onboard status\n"); - exit(1); -} - -int mavlink_onboard_main(int argc, char *argv[]) -{ - - if (argc < 2) { - warnx("missing command"); - usage(); - } - - if (!strcmp(argv[1], "start")) { - - /* this is not an error */ - if (thread_running) - errx(0, "already running"); - - thread_should_exit = false; - mavlink_task = task_spawn_cmd("mavlink_onboard", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - mavlink_thread_main, - (const char**)argv); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - while (thread_running) { - usleep(200000); - } - warnx("terminated"); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -} - diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c deleted file mode 100644 index 4658bcc1d..000000000 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_receiver.c - * MAVLink protocol message receive and dispatch - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -/* XXX trim includes */ -#include <nuttx/config.h> -#include <unistd.h> -#include <pthread.h> -#include <stdio.h> -#include <math.h> -#include <stdbool.h> -#include <fcntl.h> -#include <mqueue.h> -#include <string.h> -#include "mavlink_bridge_header.h" -#include <drivers/drv_hrt.h> -#include <time.h> -#include <float.h> -#include <unistd.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <stdlib.h> -#include <poll.h> - -#include <systemlib/param/param.h> -#include <systemlib/systemlib.h> - -#include "util.h" -#include "orb_topics.h" - -/* XXX should be in a header somewhere */ -pthread_t receive_start(int uart); - -static void handle_message(mavlink_message_t *msg); -static void *receive_thread(void *arg); - -static mavlink_status_t status; -static struct vehicle_vicon_position_s vicon_position; -static struct vehicle_command_s vcmd; -static struct offboard_control_setpoint_s offboard_control_sp; - -struct vehicle_global_position_s hil_global_pos; -struct vehicle_attitude_s hil_attitude; -orb_advert_t pub_hil_global_pos = -1; -orb_advert_t pub_hil_attitude = -1; - -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; - -static orb_advert_t offboard_control_sp_pub = -1; -static orb_advert_t vicon_position_pub = -1; - -extern bool gcs_link; - -static void -handle_message(mavlink_message_t *msg) -{ - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - //check for MAVLINK terminate command - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { - /* This is the link shutdown command, terminate mavlink */ - warnx("terminating..."); - fflush(stdout); - usleep(50000); - - /* terminate other threads and this thread */ - thread_should_exit = true; - - } else { - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - } - - /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - } - - if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); - - struct optical_flow_s f; - - f.timestamp = hrt_absolute_time(); - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - - /* check if topic is advertised */ - if (flow_pub <= 0) { - flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &f); - } - - } - - if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* Set mode on request */ - mavlink_set_mode_t new_mode; - mavlink_msg_set_mode_decode(msg, &new_mode); - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; - vcmd.target_system = new_mode.target_system; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - /* create command */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - - /* Handle Vicon position estimates */ - if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - if (vicon_position_pub <= 0) { - vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - - } else { - orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); - } - } - - /* Handle quadrotor motor setpoints */ - - if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - - if (mavlink_system.sysid < 4) { - - /* switch to a receiving link mode */ - gcs_link = false; - - /* - * rate control mode - defined by MAVLink - */ - - uint8_t ml_mode = 0; - bool ml_armed = false; - - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - /* check if topic has to be advertised */ - if (offboard_control_sp_pub <= 0) { - offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - - } else { - /* Publish */ - orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); - } - } - } - -} - - -/** - * Receive data from UART. - */ -static void * -receive_thread(void *arg) -{ - int uart_fd = *((int *)arg); - - const int timeout = 1000; - uint8_t buf[32]; - - mavlink_message_t msg; - - prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - - ssize_t nread = 0; - - while (!thread_should_exit) { - if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); - } - - /* non-blocking read. read may return negative values */ - nread = read(uart_fd, buf, sizeof(buf)); - - /* if read failed, this loop won't execute */ - for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(chan, buf[i], &msg, &status)) { - /* handle generic messages and commands */ - handle_message(&msg); - } - } - } - } - - return NULL; -} - -pthread_t -receive_start(int uart) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - - pthread_attr_setstacksize(&receiveloop_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); - return thread; -} diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk deleted file mode 100644 index a7a4980fa..000000000 --- a/src/modules/mavlink_onboard/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# MAVLink protocol to uORB interface process (XXX hack for onboard use) -# - -MODULE_COMMAND = mavlink_onboard -SRCS = mavlink.c \ - mavlink_receiver.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h deleted file mode 100644 index bbc9f6e66..000000000 --- a/src/modules/mavlink_onboard/orb_topics.h +++ /dev/null @@ -1,102 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file orb_topics.h - * Common sets of topics subscribed to or published by the MAVLink driver, - * and structures maintained by those subscriptions. - */ -#pragma once - -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/position_setpoint_triplet.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/debug_key_value.h> -#include <drivers/drv_rc_input.h> - -struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int safety_sub; - int actuators_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int spg_sub; - int debug_key_value; - int input_rc_sub; -}; - -extern struct mavlink_subscriptions mavlink_subs; - -/** Global position */ -extern struct vehicle_global_position_s global_pos; - -/** Local position */ -extern struct vehicle_local_position_s local_pos; - -/** Vehicle status */ -// extern struct vehicle_status_s v_status; - -/** RC channels */ -extern struct rc_channels_s rc; - -/** Actuator armed state */ -// extern struct actuator_armed_s armed; - -/** Worker thread starter */ -extern pthread_t uorb_receive_start(void); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 16eca8d79..c45cafc1b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -65,7 +65,6 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> #include <uORB/topics/position_setpoint_triplet.h> -#include <uORB/topics/mission_result.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> @@ -288,9 +287,9 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main task. */ - void task_main() __attribute__((noreturn)); + void task_main(); void publish_safepoints(unsigned points); @@ -395,7 +394,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), - _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -427,7 +425,6 @@ Navigator::Navigator() : _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_item, 0, sizeof(struct mission_item_s)); memset(&nav_states_str, 0, sizeof(nav_states_str)); @@ -529,13 +526,16 @@ Navigator::offboard_mission_update(bool isrotaryWing) missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); + _mission.set_current_offboard_mission_index(offboard_mission.current_index); } else { - _mission.set_current_offboard_mission_index(0); _mission.set_offboard_mission_count(0); + _mission.set_current_offboard_mission_index(0); } + + _mission.publish_mission_result(); } void @@ -545,12 +545,12 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - _mission.set_current_onboard_mission_index(onboard_mission.current_index); _mission.set_onboard_mission_count(onboard_mission.count); + _mission.set_current_onboard_mission_index(onboard_mission.current_index); } else { - _mission.set_current_onboard_mission_index(0); _mission.set_onboard_mission_count(0); + _mission.set_current_onboard_mission_index(0); } } @@ -1112,6 +1112,8 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + _mission.report_current_offboard_mission_item(); + _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1581,6 +1583,9 @@ void Navigator::on_mission_item_reached() { if (myState == NAV_STATE_MISSION) { + + _mission.report_mission_item_reached(); + if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1627,6 +1632,7 @@ Navigator::on_mission_item_reached() mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); dispatch(EVENT_READY_REQUESTED); } + _mission.publish_mission_result(); } void diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index e72caf98e..72dddebfe 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -36,13 +36,12 @@ * Helper class to access missions */ -// #include <stdio.h> -// #include <stdlib.h> -// #include <string.h> -// #include <unistd.h> - +#include <string.h> #include <stdlib.h> #include <dataman/dataman.h> +#include <systemlib/err.h> +#include <uORB/uORB.h> +#include <uORB/topics/mission_result.h> #include "navigator_mission.h" /* oddly, ERROR is not defined for c++ */ @@ -60,8 +59,11 @@ Mission::Mission() : _offboard_mission_item_count(0), _onboard_mission_item_count(0), _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE) -{} + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1) +{ + memset(&_mission_result, 0, sizeof(struct mission_result_s)); +} Mission::~Mission() { @@ -78,8 +80,16 @@ void Mission::set_current_offboard_mission_index(int new_index) { if (new_index != -1) { + warnx("specifically set to %d", new_index); _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } } + report_current_offboard_mission_item(); } void @@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index) { if (new_index != -1) { _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); } void @@ -266,4 +284,35 @@ Mission::move_to_next() default: break; } -}
\ No newline at end of file +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 15d4e86bf..2bd4da82e 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -40,6 +40,7 @@ #define NAVIGATOR_MISSION_H #include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> class __EXPORT Mission @@ -71,7 +72,9 @@ public: void move_to_next(); - void add_home_pos(struct mission_item_s *new_mission_item); + void report_mission_item_reached(); + void report_current_offboard_mission_item(); + void publish_mission_result(); private: bool current_onboard_mission_available(); @@ -92,6 +95,10 @@ private: MISSION_TYPE_ONBOARD, MISSION_TYPE_OFFBOARD, } _current_mission_type; + + int _mission_result_pub; + + struct mission_result_s _mission_result; }; -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index c99706b97..7c3921198 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -54,7 +54,8 @@ struct mission_result_s { bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index; /**< index of the mission which has been reached */ + unsigned mission_index_reached; /**< index of the mission which has been reached */ + unsigned index_current_mission; /**< index of the current mission */ }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index e5a35ff9b..40328af14 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,7 +76,7 @@ struct vehicle_attitude_s { float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ - float g_comp[3]; /**< Compensated gravity vector */ + float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ |