From 581d702478d8671faaf48c32dd7071e06d2b4254 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jan 2014 09:25:39 +0100 Subject: [PATCH 01/99] Added support for Octo Cox --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 39c8384a76..90f2e2b178 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -223,7 +223,9 @@ then # 7000 .. 7999 Hexa + # 8000 .. 8999 Octo X # 9000 .. 9999 Octo + - # 10000 .. 19999 Wide arm / H frame + # 10000 .. 10999 Wide arm / H frame + # 11000 .. 11999 Hexa Cox + # 12000 .. 12999 Octo Cox if param compare SYS_AUTOSTART 4008 8 then @@ -287,6 +289,13 @@ then sh /etc/init.d/rc.octo set MODE custom fi + + if param compare SYS_AUTOSTART 12001 + then + set MIXER /etc/mixers/FMU_octo_cox.mix + sh /etc/init.d/rc.octo + set MODE custom + fi if param compare SYS_AUTOSTART 10015 15 then From 55eaa38c651be41eee31b86330edbf27869385f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:10:03 +0100 Subject: [PATCH 02/99] Documentation cleanup in navigator --- src/modules/navigator/navigator_main.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bb7520a03f..63952be6ee 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes + * Copyright (c) 2013, 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,11 +31,18 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. * Published the mission item triplet for the position controller. + * + * @author Julian Oes + * @author Jean Cyr + * @author Anton Babushkin + * @author Thomas Gubler + * @author Lorenz Meier + * */ #include @@ -349,11 +353,11 @@ private: namespace navigator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +// /* oddly, ERROR is not defined for c++ */ +// #ifdef ERROR +// # undef ERROR +// #endif +// static const int ERROR = -1; Navigator *g_navigator; } From 03c543aba6440c25c5d349791b5a6b33914cb74c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:10:35 +0100 Subject: [PATCH 03/99] MAVLink multi-port WIP, not compiling yet --- src/modules/mavlink/mavlink.c | 802 +------- src/modules/mavlink/mavlink_bridge_header.h | 3 +- src/modules/mavlink/mavlink_main.cpp | 1819 +++++++++++++++++ src/modules/mavlink/mavlink_main.h | 255 +++ ...rb_listener.c => mavlink_orb_listener.cpp} | 199 +- src/modules/mavlink/mavlink_orb_listener.h | 143 ++ src/modules/mavlink/mavlink_parameters.c | 230 --- src/modules/mavlink/mavlink_parameters.h | 104 - src/modules/mavlink/mavlink_receiver.cpp | 12 +- .../{mavlink_hil.h => mavlink_receiver.h} | 64 +- src/modules/mavlink/module.mk | 12 +- src/modules/mavlink/orb_topics.h | 125 -- src/modules/mavlink/util.h | 3 +- src/modules/mavlink/waypoints.c | 730 ------- src/modules/mavlink/waypoints.h | 13 +- 15 files changed, 2416 insertions(+), 2098 deletions(-) create mode 100644 src/modules/mavlink/mavlink_main.cpp create mode 100644 src/modules/mavlink/mavlink_main.h rename src/modules/mavlink/{orb_listener.c => mavlink_orb_listener.cpp} (82%) create mode 100644 src/modules/mavlink/mavlink_orb_listener.h delete mode 100644 src/modules/mavlink/mavlink_parameters.c delete mode 100644 src/modules/mavlink/mavlink_parameters.h rename src/modules/mavlink/{mavlink_hil.h => mavlink_receiver.h} (65%) delete mode 100644 src/modules/mavlink/orb_topics.h delete mode 100644 src/modules/mavlink/waypoints.c diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d3c9dd2cb..aba8c98daf 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 + * 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,68 +33,49 @@ /** * @file mavlink.c - * MAVLink 1.0 protocol implementation. + * Adapter functions expected by the protocol library * * @author Lorenz Meier */ #include #include -#include #include -#include #include -#include -#include #include #include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include -#include -#include -#include -#include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" +// +// #include +// #include +// #include +// #include -#include +// #include "waypoints.h" +// #include "orb_topics.h" +// #include "mavlink_hil.h" +// #include "util.h" +// #include "waypoints.h" +// #include "mavlink_parameters.h" + +// #include /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); -__EXPORT int mavlink_main(int argc, char *argv[]); - -static int mavlink_thread_main(int argc, char *argv[]); - -/* thread state */ -volatile bool thread_should_exit = false; -static volatile bool thread_running = false; -static int mavlink_task; - -/* pthreads */ -static pthread_t receive_thread; -static pthread_t uorb_receive_thread; - -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; - mavlink_system_t mavlink_system = { 100, 50, @@ -105,353 +85,40 @@ 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 (v_status.arming_state == ARMING_STATE_ARMED - || v_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 (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; - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } - } - *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; -} - +/* + * Internal function to send the bytes through the right serial port + */ void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + int uart = -1; + + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); + break; + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); + break; + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); + break; + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); + break; + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); + break; + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); + break; + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); + break; + } + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + } /* @@ -471,362 +138,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 * 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++; - - 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 ] [-b ]\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 149efda60d..1fae3ee9d0 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 + * 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/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp new file mode 100644 index 0000000000..339030a868 --- /dev/null +++ b/src/modules/mavlink/mavlink_main.cpp @@ -0,0 +1,1819 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "mavlink_bridge_header.h" +#include "mavlink_parameters.h" +#include +#include "math.h" /* isinf / isnan checks */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/** + * mavlink app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); + + + +namespace mavlink +{ + + Mavlink *g_mavlink; +} + +Mavlink::Mavlink(const char *port, unsigned baud_rate) : + + _task_should_exit(false), + _mavlink_task(-1), + _mavlink_fd(-1), + _mavlink_incoming_fd(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), + _mavlink_hil_enabled(false) + // _params_sub(-1) +{ + // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + +} + +Mavlink::~Mavlink() +{ + if (_mavlink_task != -1) { + + /* task wakes up every 100ms 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) { + task_delete(_mavlink_task); + break; + } + } while (_mavlink_task != -1); + } +} + +int Mavlink::instance_count() +{ + /* note: a local buffer count will help if this ever is called often */ + Mavlink* inst = _head; + unsigned inst_index = 0; + while (inst->_head != nullptr) { + inst = inst->_head; + inst_index++; + } + + return inst_index + 1; +} + +Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) +{ + Mavlink* inst = new Mavlink(port, baud_rate); + Mavlink* parent = _head; + while (parent->_head != nullptr) + parent = parent->_head; + + /* now parent points to a null pointer, fill it */ + parent->_head = inst; + + return inst; +} + +Mavlink* Mavlink::get_instance(unsigned instance) +{ + Mavlink* inst = _head; + unsigned inst_index = 0; + while (inst->_head != nullptr && inst_index < instance) { + inst = inst->_head; + inst_index++; + } + + if (inst_index < instance) { + inst = nullptr; + } + + return inst; +} + +void +Mavlink::parameters_update() +{ + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + // param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); + +} + +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; + +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); + +/* Allocate storage space for waypoints */ +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; + +bool mavlink_hil_enabled = false; + +/* protocol interface */ +static int uart; +static int baudrate; +bool gcs_link = true; + +/* interface mode */ +static enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD +} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; + +static struct mavlink_logbuffer lb; + +static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + +static const struct file_operations mavlink_fops = { + .ioctl = mavlink_dev_ioctl +}; + +static int +mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + 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; + } +} + +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; + } +} + +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; +} + +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 (v_status.arming_state == ARMING_STATE_ARMED + || v_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 (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; + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (control_mode.nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } + } + *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; +} + +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; + } +} + +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; + } + } +} + +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 +Mavlink::task_main_trampoline(int argc, char *argv[]) +{ + mavlink::g_mavlink->task_main(); +} + +void +Mavlink::task_main() +{ + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* initialize logging device */ + // YYY + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + mavlink_log_info(_mavlink_fd, "[mavlink] started"); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* parameters updated */ + if (fds[0].revents & POLLIN) { + parameters_update(); + } + + + + + + /* 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 * 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++; + + 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; + + + + + + + + perf_end(_loop_perf); + } + + warnx("exiting."); + + _mavlink_task = -1; + _exit(0); +} + +int +Mavlink::start() +{ + ASSERT(_mavlink_task == -1); + + /* start the task */ + char buf[32]; + sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + + _mavlink_task = task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Mavlink::task_main_trampoline, + nullptr); + + // while (!this->is_running()) { + // usleep(200); + // } + + if (_mavlink_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +void +Mavlink::status() +{ + warnx("Running"); +} + +static void usage() +{ + errx(1, "usage: mavlink {start|stop|status}"); +} + +int mavlink_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + Mavlink *instance = Mavlink::new_instance("/dev/ttyS0", 57600); + + if (mavlink::g_mavlink == nullptr) + mavlink::g_mavlink = instance; + + // if (mavlink::g_mavlink != nullptr) { + // errx(1, "already running"); + // } + + // mavlink::g_mavlink = new Mavlink; + + // if (mavlink::g_mavlink == nullptr) { + // errx(1, "alloc failed"); + // } + + // if (OK != mavlink::g_mavlink->start()) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + // err(1, "start failed"); + // } + + return 0; + } + + // if (mavlink::g_mavlink == nullptr) + // errx(1, "not running"); + + // if (!strcmp(argv[1], "stop")) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + + // } else if (!strcmp(argv[1], "status")) { + // mavlink::g_mavlink->status(); + + // } 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 0000000000..8d6f0bd6f7 --- /dev/null +++ b/src/modules/mavlink/mavlink_main.h @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Mavlink +{ +public: + /** + * Constructor + */ + Mavlink(const char *port, unsigned baud_rate); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the mavlink status. + */ + void status(); + + static int instance_count(); + + static Mavlink* new_instance(const char *port, unsigned baud_rate); + + static Mavlink* get_instance(unsigned instance); + + 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 control_mode_sub; + }; + + struct mavlink_subscriptions subs; + + /** Global position */ + struct vehicle_global_position_s global_pos; + /** Local position */ + struct vehicle_local_position_s local_pos; + /** navigation capabilities */ + struct navigation_capabilities_s nav_cap; + /** Vehicle status */ + struct vehicle_status_s v_status; + /** Vehicle control mode */ + struct vehicle_control_mode_s control_mode; + /** RC channels */ + struct rc_channels_s rc; + /** Actuator armed state */ + struct actuator_armed_s armed; + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _mavlink_task; /**< task handle for sensor task */ + + int _mavlink_fd; + int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /* states */ + bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ + static Mavlink* _head; + + int _params_sub; + + orb_advert_t mission_pub = -1; +struct mission_s mission; + +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + + /** + * Update our local parameter cache. + */ + void parameters_update(); + + /** + * 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_on_off(bool hil_enabled); + + + /** + * 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); + + /** + * Shim for calling task_main from task_create. + */ + void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); + +}; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/mavlink_orb_listener.cpp similarity index 82% rename from src/modules/mavlink/orb_listener.c rename to src/modules/mavlink/mavlink_orb_listener.cpp index 6e177bc4d9..3edad0f16f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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,7 +32,7 @@ ****************************************************************************/ /** - * @file orb_listener.c + * @file orb_listener.cpp * Monitors ORB topics and sends update messages as appropriate. * * @author Lorenz Meier @@ -58,44 +57,11 @@ #include -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "util.h" +#include "mavlink_orb_listener.h" +#include "mavlink_main.h" +//#include "util.h" -extern bool gcs_link; - -struct vehicle_global_position_s global_pos; -struct vehicle_local_position_s local_pos; -struct navigation_capabilities_s nav_cap; -struct vehicle_status_s v_status; -struct vehicle_control_mode_s control_mode; -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); + void *uorb_receive_thread(void *arg); struct listener { void (*callback)(const struct listener *l); @@ -105,30 +71,28 @@ struct listener { 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 void l_control_mode(const struct listener *l); -static const struct listener listeners[] = { +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); +} + +MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : + + _task_should_exit(false), + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), + _mavlink(parent), + _n_listeners(0) +{ + 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}, @@ -156,23 +120,12 @@ static const struct listener listeners[] = { {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; -static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); + _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) +MavlinkOrbListener::l_sensor_combined(const struct listener *l) { struct sensor_combined_s raw; @@ -213,7 +166,7 @@ l_sensor_combined(const struct listener *l) } if (gcs_link) - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), 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], @@ -227,14 +180,14 @@ l_sensor_combined(const struct listener *l) } void -l_vehicle_attitude(const struct listener *l) +MavlinkOrbListener::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, + mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, att.roll, att.pitch, @@ -250,12 +203,12 @@ l_vehicle_attitude(const struct listener *l) float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); 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.vz); + mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } /* send quaternion values if it exists */ if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, + mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, att.q[0], att.q[1], @@ -271,7 +224,7 @@ l_vehicle_attitude(const struct listener *l) } void -l_vehicle_gps_position(const struct listener *l) +MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) { struct vehicle_gps_position_s gps; @@ -282,7 +235,7 @@ l_vehicle_gps_position(const struct listener *l) float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, + mavlink_msg_gps_raw_int_send(_mavlink->get_mavlink_chan(), gps.timestamp_position, gps.fix_type, gps.lat, @@ -296,7 +249,7 @@ l_vehicle_gps_position(const struct listener *l) /* update SAT info every 10 seconds */ if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, + mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(), gps.satellites_visible, gps.satellite_prn, gps.satellite_used, @@ -309,7 +262,7 @@ l_vehicle_gps_position(const struct listener *l) } void -l_vehicle_status(const struct listener *l) +MavlinkOrbListener::l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); @@ -337,7 +290,7 @@ l_vehicle_status(const struct listener *l) } void -l_rc_channels(const struct listener *l) +MavlinkOrbListener::l_rc_channels(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -345,7 +298,7 @@ l_rc_channels(const struct listener *l) } void -l_input_rc(const struct listener *l) +MavlinkOrbListener::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); @@ -373,12 +326,12 @@ l_input_rc(const struct listener *l) } void -l_global_position(const struct listener *l) +MavlinkOrbListener::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, + mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), global_pos.timestamp / 1000, global_pos.lat, global_pos.lon, @@ -391,13 +344,13 @@ l_global_position(const struct listener *l) } void -l_local_position(const struct listener *l) +MavlinkOrbListener::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, + mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(), local_pos.timestamp / 1000, local_pos.x, local_pos.y, @@ -408,7 +361,7 @@ l_local_position(const struct listener *l) } void -l_global_position_setpoint(const struct listener *l) +MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { struct mission_item_triplet_s triplet; orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); @@ -422,7 +375,7 @@ l_global_position_setpoint(const struct listener *l) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), coordinate_frame, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), @@ -431,7 +384,7 @@ l_global_position_setpoint(const struct listener *l) } void -l_local_position_setpoint(const struct listener *l) +MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) { struct vehicle_local_position_setpoint_s local_sp; @@ -439,7 +392,7 @@ l_local_position_setpoint(const struct listener *l) 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, + mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(), MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -448,7 +401,7 @@ l_local_position_setpoint(const struct listener *l) } void -l_attitude_setpoint(const struct listener *l) +MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) { struct vehicle_attitude_setpoint_s att_sp; @@ -456,7 +409,7 @@ l_attitude_setpoint(const struct listener *l) 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, + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(), att_sp.timestamp / 1000, att_sp.roll_body, att_sp.pitch_body, @@ -465,7 +418,7 @@ l_attitude_setpoint(const struct listener *l) } void -l_vehicle_rates_setpoint(const struct listener *l) +MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) { struct vehicle_rates_setpoint_s rates_sp; @@ -473,7 +426,7 @@ l_vehicle_rates_setpoint(const struct listener *l) 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, + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(), rates_sp.timestamp / 1000, rates_sp.roll, rates_sp.pitch, @@ -482,7 +435,7 @@ l_vehicle_rates_setpoint(const struct listener *l) } void -l_actuator_outputs(const struct listener *l) +MavlinkOrbListener::l_actuator_outputs(const struct listener *l) { struct actuator_outputs_s act_outputs; @@ -497,7 +450,7 @@ l_actuator_outputs(const struct listener *l) 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, + mavlink_msg_servo_output_raw_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, l->arg /* port number - needs GCS support */, /* QGC has port number support already */ act_outputs.output[0], @@ -583,13 +536,13 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +MavlinkOrbListener::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) +MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) { struct manual_control_setpoint_s man_control; @@ -597,7 +550,7 @@ l_manual_control_setpoint(const struct listener *l) 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_msg_manual_control_send(_mavlink->get_mavlink_chan(), mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -607,25 +560,25 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +MavlinkOrbListener::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, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl0 ", actuators_0.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl1 ", actuators_0.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl2 ", actuators_0.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl3 ", actuators_0.control[3]); @@ -633,7 +586,7 @@ l_vehicle_attitude_controls(const struct listener *l) } void -l_debug_key_value(const struct listener *l) +MavlinkOrbListener::l_debug_key_value(const struct listener *l) { struct debug_key_value_s debug; @@ -642,46 +595,46 @@ l_debug_key_value(const struct listener *l) /* Enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, debug.key, debug.value); } void -l_optical_flow(const struct listener *l) +MavlinkOrbListener::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, + mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), 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) +MavlinkOrbListener::l_home(const struct listener *l) { struct home_position_s home; orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); } void -l_airspeed(const struct listener *l) +MavlinkOrbListener::l_airspeed(const struct listener *l) { orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); } void -l_nav_cap(const struct listener *l) +MavlinkOrbListener::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, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), hrt_absolute_time() / 1000, "turn dist", nav_cap.turn_distance); @@ -689,7 +642,7 @@ l_nav_cap(const struct listener *l) } void -l_control_mode(const struct listener *l) +MavlinkOrbListener::l_control_mode(const struct listener *l) { orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); @@ -725,9 +678,9 @@ uorb_receive_thread(void *arg) * * Might want to invoke each listener once to set initial state. */ - struct pollfd fds[n_listeners]; + struct pollfd fds[_n_listeners]; - for (unsigned i = 0; i < n_listeners; i++) { + for (unsigned i = 0; i < _n_listeners; i++) { fds[i].fd = *listeners[i].subp; fds[i].events = POLLIN; @@ -737,7 +690,7 @@ uorb_receive_thread(void *arg) while (!thread_should_exit) { - int poll_ret = poll(fds, n_listeners, timeout); + int poll_ret = poll(fds, _n_listeners, timeout); /* handle the poll result */ if (poll_ret == 0) { @@ -748,7 +701,7 @@ uorb_receive_thread(void *arg) } else { - for (unsigned i = 0; i < n_listeners; i++) { + for (unsigned i = 0; i < _n_listeners; i++) { if (fds[i].revents & POLLIN) listeners[i].callback(&listeners[i]); } diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h new file mode 100644 index 0000000000..32a174fcd0 --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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_orb_listener.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + */ + +#pragma once + +class Mavlink; + +class MavlinkOrbListener +{ +public: + /** + * Constructor + */ + MavlinkOrbListener(Mavlink* parent); + + /** + * Destructor, also kills the mavlinks task. + */ + ~MavlinkOrbListener(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the mavlink status. + */ + void status(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Mavlink* _mavlink; + + unsigned _n_listeners; + + /** + * Shim for calling task_main from task_create. + */ + void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); + + void l_sensor_combined(const struct listener *l); + void l_vehicle_attitude(const struct listener *l); + void l_vehicle_gps_position(const struct listener *l); + void l_vehicle_status(const struct listener *l); + void l_rc_channels(const struct listener *l); + void l_input_rc(const struct listener *l); + void l_global_position(const struct listener *l); + void l_local_position(const struct listener *l); + void l_global_position_setpoint(const struct listener *l); + void l_local_position_setpoint(const struct listener *l); + void l_attitude_setpoint(const struct listener *l); + void l_actuator_outputs(const struct listener *l); + void l_actuator_armed(const struct listener *l); + void l_manual_control_setpoint(const struct listener *l); + void l_vehicle_attitude_controls(const struct listener *l); + void l_debug_key_value(const struct listener *l); + void l_optical_flow(const struct listener *l); + void l_vehicle_rates_setpoint(const struct listener *l); + void l_home(const struct listener *l); + void l_airspeed(const struct listener *l); + void l_nav_cap(const struct listener *l); + void l_control_mode(const struct listener *l); + + struct vehicle_global_position_s global_pos; + struct vehicle_local_position_s local_pos; + struct navigation_capabilities_s nav_cap; + struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; + 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; + + int status_sub; + int rc_sub; + + unsigned int sensors_raw_counter; + unsigned int attitude_counter; + unsigned int gps_counter; + + /* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ + uint64_t last_sensor_timestamp; + + hrt_abstime last_sent_vfr = 0; + +}; diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c deleted file mode 100644 index 18ca7a854b..0000000000 --- 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 - * - * 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 - */ - -#include "mavlink_bridge_header.h" -#include "mavlink_parameters.h" -#include -#include "math.h" /* isinf / isnan checks */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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 b1e38bcc85..0000000000 --- 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 - * - * 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 - */ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - - -#include -#include -#include - -/** - * 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_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 771989430a..6881a22802 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 + * 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,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_receiver.c + * @file mavlink_receiver.cpp * MAVLink protocol message receive and dispatch * * @author Lorenz Meier @@ -79,12 +78,9 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "waypoints.h" #include "orb_topics.h" -#include "mavlink_hil.h" #include "mavlink_parameters.h" #include "util.h" -extern bool gcs_link; - __END_DECLS /* XXX should be in a header somewhere */ @@ -272,7 +268,7 @@ handle_message(mavlink_message_t *msg) if (mavlink_system.sysid < 4) { /* switch to a receiving link mode */ - gcs_link = false; + _mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); /* * rate control mode - defined by MAVLink @@ -367,7 +363,7 @@ handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - if (mavlink_hil_enabled) { + if (_mavlink->hil_enabled()) { uint64_t timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_receiver.h similarity index 65% rename from src/modules/mavlink/mavlink_hil.h rename to src/modules/mavlink/mavlink_receiver.h index 744ed7d942..88ae4b1103 100644 --- a/src/modules/mavlink/mavlink_hil.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * 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,57 @@ ****************************************************************************/ /** - * @file mavlink_hil.h - * Hardware-in-the-loop simulation support. + * @file mavlink_orb_listener.h + * MAVLink 1.0 uORB listener definition + * + * @author Lorenz Meier */ #pragma once -extern bool mavlink_hil_enabled; +class Mavlink; -/** - * 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); +class MavlinkReceiver +{ +public: + /** + * Constructor + */ + MavlinkReceiver(Mavlink *parent); + + /** + * Destructor, also kills the mavlinks task. + */ + ~MavlinkReceiver(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the mavlink status. + */ + void status(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Mavlink* _mavlink; + + /** + * Shim for calling task_main from task_create. + */ + void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); + +}; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 89a097c245..2a005565e0 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,10 @@ # 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_listener.cpp \ + waypoints.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h deleted file mode 100644 index 4d428406ac..0000000000 --- a/src/modules/mavlink/orb_topics.h +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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 control_mode_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; - -/** navigation capabilities */ -extern struct navigation_capabilities_s nav_cap; - -/** Vehicle status */ -extern struct vehicle_status_s v_status; - -/** Vehicle control mode */ -extern struct vehicle_control_mode_s control_mode; - -/** RC channels */ -extern struct rc_channels_s rc; - -/** Actuator armed state */ -extern struct actuator_armed_s armed; - -/** Worker thread starter */ -extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index 5e5ee8261c..5ca9a085d5 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 + * 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 168666d4e1..0000000000 --- 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 - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - * 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 -#include -#include -#include -#include "mavlink_bridge_header.h" -#include "waypoints.h" -#include "util.h" -#include -#include -#include -#include -#include -#include - -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 f8b58c7d92..532eff7aa1 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 - * @author Thomas Gubler - * @author Julian Oes + * 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 + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes */ #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_ */ From ac77fe9c27d7253b01805ff94d3e0f8e21017709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 18:40:02 +0100 Subject: [PATCH 04/99] WIP state on getting MAVLink as a class, much of the work done, but does not compile yet --- src/modules/mavlink/mavlink.c | 36 --- src/modules/mavlink/mavlink_main.cpp | 262 ++++++++++--------- src/modules/mavlink/mavlink_main.h | 153 ++++++++++- src/modules/mavlink/mavlink_orb_listener.cpp | 6 +- src/modules/mavlink/mavlink_orb_listener.h | 19 +- src/modules/mavlink/mavlink_receiver.cpp | 90 +++---- src/modules/mavlink/mavlink_receiver.h | 77 +++++- 7 files changed, 400 insertions(+), 243 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index aba8c98daf..b143e62f0c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -85,42 +85,6 @@ mavlink_system_t mavlink_system = { 0 }; // System ID, 1-255, Component/Subsystem ID, 1-255 -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - int uart = -1; - - switch (channel) { - case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); - break; - case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); - break; - case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); - break; - case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); - break; - case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); - break; - case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); - break; - case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); - break; - } - - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); - -} - /* * Internal function to give access to the channel status for each channel */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 339030a868..1c7986cbb8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -70,10 +71,10 @@ #include #include #include +#include #include "mavlink_bridge_header.h" -#include "mavlink_parameters.h" #include #include "math.h" /* isinf / isnan checks */ #include @@ -87,7 +88,9 @@ #include #include #include -#include +#include "mavlink_main.h" +#include "mavlink_orb_listener.h" +#include "mavlink_receiver.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -102,7 +105,49 @@ static const int ERROR = -1; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +/* + * Internal function to send the bytes through the right serial port + */ +void +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) +{ + int uart = -1; + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); + break; + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); + break; + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); + break; + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); + break; + #ifdef MAVLINK_COMM_4 + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); + break; + #endif + #ifdef MAVLINK_COMM_5 + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); + break; + #endif + #ifdef MAVLINK_COMM_6 + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); + break; + #endif + } + + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + +} + +static void usage(void); namespace mavlink { @@ -122,6 +167,8 @@ Mavlink::Mavlink(const char *port, unsigned baud_rate) : _mavlink_hil_enabled(false) // _params_sub(-1) { + wpm = &wpm_s; + fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); } @@ -191,6 +238,15 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +int Mavlink::get_uart_fd(unsigned index) +{ + Mavlink* inst = get_instance(index); + if (inst) + return inst->_mavlink_fd; + + return -1; +} + void Mavlink::parameters_update() { @@ -202,48 +258,12 @@ Mavlink::parameters_update() } -/* XXX not widely used */ -uint8_t chan = MAVLINK_COMM_0; - -/* XXX probably should be in a header... */ -extern pthread_t receive_start(int uart); - -/* Allocate storage space for waypoints */ -static mavlink_wpm_storage wpm_s; -mavlink_wpm_storage *wpm = &wpm_s; - -bool mavlink_hil_enabled = false; - -/* protocol interface */ -static int uart; -static int baudrate; -bool gcs_link = true; - -/* interface mode */ -static enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD -} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; - -static struct mavlink_logbuffer lb; - -static void mavlink_update_system(void); -static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -static void usage(void); -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - /**************************************************************************** * MAVLink text message logger ****************************************************************************/ -static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations mavlink_fops = { - .ioctl = mavlink_dev_ioctl -}; - -static int -mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +int +Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) { static unsigned int total_counter = 0; @@ -252,10 +272,11 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; + printf("logmsg: %s\n", txt); + //struct mavlink_logmessage msg; + //strncpy(msg.text, txt, sizeof(msg.text)); + //mavlink_logbuffer_write(&lb, &msg); + //total_counter++; return OK; } @@ -264,7 +285,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } -void mavlink_update_system(void) +void Mavlink::mavlink_update_system(void) { static bool initialized = false; static param_t param_system_id; @@ -301,7 +322,7 @@ void mavlink_update_system(void) } } -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ int speed; @@ -398,7 +419,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf } int -set_hil_on_off(bool hil_enabled) +Mavlink::set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -427,13 +448,13 @@ set_hil_on_off(bool hil_enabled) hil_rate_interval = 5; } - orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + orb_set_interval(subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; - orb_set_interval(mavlink_subs.spa_sub, 200); + orb_set_interval(subs.spa_sub, 200); } else { ret = ERROR; @@ -443,7 +464,7 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ *mavlink_base_mode = 0; @@ -518,7 +539,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; @@ -552,7 +573,7 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->actuators_sub, min_interval); orb_set_interval(subs->actuators_effective_sub, min_interval); orb_set_interval(subs->spa_sub, min_interval); - orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); + orb_set_interval(subs->rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: @@ -575,34 +596,19 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m extern mavlink_system_t mavlink_system; -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); - -/** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ -static unsigned int mavlink_param_queue_index = 0; - -/** - * Callback for param interface. - */ -void mavlink_pm_callback(void *arg, param_t param); - -void mavlink_pm_callback(void *arg, param_t param) +void Mavlink::mavlink_pm_callback(void *arg, param_t param) { - mavlink_pm_send_param(param); + //mavlink_pm_send_param(param); usleep(*(unsigned int *)arg); } -void mavlink_pm_send_all_params(unsigned int delay) +void Mavlink::mavlink_pm_send_all_params(unsigned int delay) { unsigned int dbuf = delay; param_foreach(&mavlink_pm_callback, &dbuf, false); } -int mavlink_pm_queued_send() +int Mavlink::mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); @@ -614,22 +620,22 @@ int mavlink_pm_queued_send() } } -void mavlink_pm_start_queued_send() +void Mavlink::mavlink_pm_start_queued_send() { mavlink_param_queue_index = 0; } -int mavlink_pm_send_param_for_index(uint16_t index) +int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) { return mavlink_pm_send_param(param_for_index(index)); } -int mavlink_pm_send_param_for_name(const char *name) +int Mavlink::mavlink_pm_send_param_for_name(const char *name) { return mavlink_pm_send_param(param_find(name)); } -int mavlink_pm_send_param(param_t param) +int Mavlink::mavlink_pm_send_param(param_t param) { if (param == PARAM_INVALID) return 1; @@ -679,11 +685,11 @@ int mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - ret = mavlink_missionlib_send_message(&tx_msg); - return ret; + mavlink_missionlib_send_message(&tx_msg); + return OK; } -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { @@ -748,7 +754,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } -void publish_mission() +void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (mission_pub < 0) { @@ -759,7 +765,7 @@ void publish_mission() } } -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { @@ -797,7 +803,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = mavlink_mission_item->command; + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; @@ -807,7 +813,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli return OK; } -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { if (mission_item->altitude_is_relative) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL; @@ -838,7 +844,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio return OK; } -void mavlink_wpm_init(mavlink_wpm_storage *state) +void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -854,7 +860,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) /* * @brief Sends an waypoint ack message */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) { mavlink_message_t msg; mavlink_mission_ack_t wpa; @@ -878,7 +884,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) * * @param seq The waypoint sequence number the MAV should fly to. */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) { if (seq < wpm->size) { mavlink_message_t msg; @@ -897,7 +903,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) } } -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) { mavlink_message_t msg; mavlink_mission_count_t wpc; @@ -912,7 +918,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { struct mission_item_s mission_item; @@ -946,7 +952,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) } } -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < wpm->max_size) { mavlink_message_t msg; @@ -972,7 +978,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s * * @param seq The waypoint sequence number the MAV has reached. */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) { mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; @@ -986,7 +992,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) } -void mavlink_waypoint_eventloop(uint64_t now) +void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -1002,7 +1008,7 @@ void mavlink_waypoint_eventloop(uint64_t now) } -void mavlink_wpm_message_handler(const mavlink_message_t *msg) +void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) { uint64_t now = hrt_absolute_time(); @@ -1377,7 +1383,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg) } void -mavlink_missionlib_send_message(mavlink_message_t *msg) +Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); @@ -1387,7 +1393,7 @@ mavlink_missionlib_send_message(mavlink_message_t *msg) int -mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; @@ -1419,11 +1425,11 @@ mavlink_missionlib_send_gcs_string(const char *string) void Mavlink::task_main_trampoline(int argc, char *argv[]) { - mavlink::g_mavlink->task_main(); + mavlink::g_mavlink->task_main(argc, argv); } -void -Mavlink::task_main() +int +Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ warnx("Initializing.."); @@ -1529,16 +1535,18 @@ Mavlink::task_main() err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* Initialize system properties */ mavlink_update_system(); /* start the MAVLink receiver */ - receive_thread = receive_start(uart); + MavlinkReceiver rcv(this); + receive_thread = rcv.receive_start(uart); /* start the ORB receiver */ - uorb_receive_thread = uorb_receive_start(); + MavlinkOrbListener listener(this); + uorb_receive_thread = listener.uorb_receive_start(); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1546,57 +1554,57 @@ Mavlink::task_main() /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 230400) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8d6f0bd6f7..244af04a67 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -73,6 +73,70 @@ #include #include + +#include + + // FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 15 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; + int current_dataman_id; +}; + class Mavlink { public: @@ -104,6 +168,8 @@ public: static Mavlink* get_instance(unsigned instance); + static int get_uart_fd(unsigned index); + struct mavlink_subscriptions { int sensor_sub; int att_sub; @@ -149,12 +215,19 @@ public: /** Actuator armed state */ struct actuator_armed_s armed; +protected: + /** + * Pointer to the default cdev file operations table; useful for + * registering clone devices etc. + */ + struct file_operations fops; + int _mavlink_fd; + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _mavlink_task; /**< task handle for sensor task */ - int _mavlink_fd; int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -165,10 +238,45 @@ private: int _params_sub; - orb_advert_t mission_pub = -1; -struct mission_s mission; + orb_advert_t mission_pub; + struct mission_s mission; + uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + bool thread_running; + bool thread_should_exit; -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + uint8_t mavlink_wpm_comp_id; + mavlink_channel_t chan; + +// XXX probably should be in a header... +// extern pthread_t receive_start(int uart); + + pthread_t receive_thread; + pthread_t uorb_receive_thread; + + /* Allocate storage space for waypoints */ + mavlink_wpm_storage wpm_s; + mavlink_wpm_storage *wpm; + + bool verbose; + bool mavlink_hil_enabled; + int uart; + int baudrate; + bool gcs_link; + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int mavlink_param_queue_index; + + /* interface mode */ + enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD + } mavlink_link_mode; + + struct mavlink_logbuffer lb; + bool mavlink_link_termination_allowed; /** * Update our local parameter cache. @@ -240,7 +348,38 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(void); + void mavlink_pm_start_queued_send(); + + void mavlink_update_system(); + + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); + void mavlink_wpm_send_waypoint_current(uint16_t seq); + void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); + void mavlink_wpm_init(mavlink_wpm_storage *state); + int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + void publish_mission(); + + void mavlink_missionlib_send_message(mavlink_message_t *msg); + int mavlink_missionlib_send_gcs_string(const char *string); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + + void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Callback for param interface. + */ + static void mavlink_pm_callback(void *arg, param_t param); + + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** * Shim for calling task_main from task_create. @@ -248,8 +387,8 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main mavlink task. */ - void task_main() __attribute__((noreturn)); + int task_main(int argc, char *argv[]) __attribute__((noreturn)); }; diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 3edad0f16f..e2675dfa71 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -661,8 +661,8 @@ MavlinkOrbListener::l_control_mode(const struct listener *l) mavlink_state); } -static void * -uorb_receive_thread(void *arg) +void * +MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); @@ -712,7 +712,7 @@ uorb_receive_thread(void *arg) } pthread_t -uorb_receive_start(void) +MavlinkOrbListener::uorb_receive_start(void) { /* --- SENSORS RAW VALUE --- */ mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 32a174fcd0..29e081b360 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -55,18 +55,21 @@ public: */ ~MavlinkOrbListener(); - /** - * Start the mavlink task. - * - * @return OK on success. - */ - int start(); + // * + // * Start the mavlink task. + // * + // * @return OK on success. + + // int start(); /** * Display the mavlink status. */ void status(); + pthread_t uorb_receive_start(void); + void * uorb_receive_thread(void *arg); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -122,8 +125,6 @@ private: struct vehicle_attitude_s att; struct airspeed_s airspeed; - struct mavlink_subscriptions mavlink_subs; - int status_sub; int rc_sub; @@ -138,6 +139,6 @@ private: */ uint64_t last_sensor_timestamp; - hrt_abstime last_sent_vfr = 0; + hrt_abstime last_sent_vfr; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6881a22802..ab4074558a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -76,61 +76,42 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_parameters.h" +// #include "waypoints.h" +#include "mavlink_receiver.h" +#include "mavlink_main.h" #include "util.h" __END_DECLS -/* XXX should be in a header somewhere */ -extern "C" pthread_t receive_start(int uart); +void MavlinkReceiver::MavlinkReceiver() : + pub_hil_global_pos(-1), + pub_hil_local_pos(-1), + pub_hil_attitude(-1), + pub_hil_gps(-1), + pub_hil_sensors(-1), + pub_hil_gyro(-1), + pub_hil_accel(-1), + pub_hil_mag(-1), + pub_hil_baro(-1), + pub_hil_airspeed(-1), + pub_hil_battery(-1), + hil_counter(0), + hil_frames(0), + old_timestamp(0), + cmd_pub(-1), + flow_pub(-1), + offboard_control_sp_pub(-1), + vicon_position_pub(-1), + telemetry_status_pub(-1), + lat0(0), + lon0(0), + alt0(0) +{ -static void handle_message(mavlink_message_t *msg); -static void *receive_thread(void *arg); +} -static mavlink_status_t status; -static struct vehicle_vicon_position_s vicon_position; -static struct vehicle_command_s vcmd; -static struct offboard_control_setpoint_s offboard_control_sp; - -struct vehicle_global_position_s hil_global_pos; -struct vehicle_local_position_s hil_local_pos; -struct vehicle_attitude_s hil_attitude; -struct vehicle_gps_position_s hil_gps; -struct sensor_combined_s hil_sensors; -struct battery_status_s hil_battery_status; -static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t pub_hil_local_pos = -1; -static orb_advert_t pub_hil_attitude = -1; -static orb_advert_t pub_hil_gps = -1; -static orb_advert_t pub_hil_sensors = -1; -static orb_advert_t pub_hil_gyro = -1; -static orb_advert_t pub_hil_accel = -1; -static orb_advert_t pub_hil_mag = -1; -static orb_advert_t pub_hil_baro = -1; -static orb_advert_t pub_hil_airspeed = -1; -static orb_advert_t pub_hil_battery = -1; - -/* packet counter */ -static int hil_counter = 0; -static int hil_frames = 0; -static uint64_t old_timestamp = 0; - -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; - -static orb_advert_t offboard_control_sp_pub = -1; -static orb_advert_t vicon_position_pub = -1; -static orb_advert_t telemetry_status_pub = -1; - -// variables for HIL reference position -static int32_t lat0 = 0; -static int32_t lon0 = 0; -static double alt0 = 0; - -static void -handle_message(mavlink_message_t *msg) +void +MavlinkReceiver::handle_message(mavlink_message_t *msg) { if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { @@ -804,8 +785,8 @@ handle_message(mavlink_message_t *msg) /** * Receive data from UART. */ -static void * -receive_thread(void *arg) +void * +MavlinkReceiver::receive_thread(void *arg) { int uart_fd = *((int *)arg); @@ -851,8 +832,13 @@ receive_thread(void *arg) return NULL; } +void MavlinkReceiver::print_status() +{ + +} + pthread_t -receive_start(int uart) +MavlinkReceiver::receive_start(int uart) { pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 88ae4b1103..556b6f8ad6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -40,6 +40,36 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + class Mavlink; class MavlinkReceiver @@ -65,7 +95,9 @@ public: /** * Display the mavlink status. */ - void status(); + void print_status(); + + pthread_t receive_start(int uart); private: @@ -75,14 +107,41 @@ private: Mavlink* _mavlink; - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - /** - * Main sensor collection task. - */ - void task_main() __attribute__((noreturn)); + void handle_message(mavlink_message_t *msg); + void *receive_thread(void *arg); + + mavlink_status_t status; + struct vehicle_vicon_position_s vicon_position; + struct vehicle_command_s vcmd; + struct offboard_control_setpoint_s offboard_control_sp; + struct vehicle_global_position_s hil_global_pos; + struct vehicle_local_position_s hil_local_pos; + struct vehicle_attitude_s hil_attitude; + struct vehicle_gps_position_s hil_gps; + struct sensor_combined_s hil_sensors; + struct battery_status_s hil_battery_status; + orb_advert_t pub_hil_global_pos; + orb_advert_t pub_hil_local_pos; + orb_advert_t pub_hil_attitude; + orb_advert_t pub_hil_gps; + orb_advert_t pub_hil_sensors; + orb_advert_t pub_hil_gyro; + orb_advert_t pub_hil_accel; + orb_advert_t pub_hil_mag; + orb_advert_t pub_hil_baro; + orb_advert_t pub_hil_airspeed; + orb_advert_t pub_hil_battery; + int hil_counter; + int hil_frames; + uint64_t old_timestamp; + orb_advert_t cmd_pub; + orb_advert_t flow_pub; + orb_advert_t offboard_control_sp_pub; + orb_advert_t vicon_position_pub; + orb_advert_t telemetry_status_pub; + int32_t lat0; + int32_t lon0; + double alt0; }; From 9c355d280eb379c72df636c1d47e5b14ae3e3e6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 15:13:14 +0100 Subject: [PATCH 05/99] Merged beta into mavlink rework branch --- Documentation/Doxyfile | 2 +- .../init.d/1000_rc_fw_easystar.hil | 58 +- .../px4fmu_common/init.d/10015_tbs_discovery | 85 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 102 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 105 -- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 42 + .../px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 105 +- .../init.d/1004_rc_fw_Rascal110.hil | 69 +- ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm | 37 + ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 61 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 48 +- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 48 +- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 84 +- ROMFS/px4fmu_common/init.d/3031_io_phantom | 85 -- ROMFS/px4fmu_common/init.d/3031_phantom | 44 + ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 89 +- ROMFS/px4fmu_common/init.d/3033_io_wingwing | 84 -- ROMFS/px4fmu_common/init.d/3033_wingwing | 43 + ROMFS/px4fmu_common/init.d/3034_fx79 | 43 + ROMFS/px4fmu_common/init.d/3034_io_fx79 | 84 -- ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 100 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 91 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 31 + ROMFS/px4fmu_common/init.d/40_io_segway | 51 - ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 + ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 + ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 76 -- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 + ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 + ROMFS/px4fmu_common/init.d/800_sdlogger | 51 - ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 + ROMFS/px4fmu_common/init.d/cmp_test | 9 - ROMFS/px4fmu_common/init.d/rc.autostart | 195 ++++ .../init.d/rc.custom_dji_f330_mkblctrl | 113 -- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 -- ROMFS/px4fmu_common/init.d/rc.fixedwing | 34 - ROMFS/px4fmu_common/init.d/rc.fw_apps | 19 + ROMFS/px4fmu_common/init.d/rc.hexa | 94 -- ROMFS/px4fmu_common/init.d/rc.interface | 72 ++ ROMFS/px4fmu_common/init.d/rc.io | 36 +- ROMFS/px4fmu_common/init.d/rc.logging | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 24 + ROMFS/px4fmu_common/init.d/rc.mc_interface | 49 - ROMFS/px4fmu_common/init.d/rc.multirotor | 39 - ROMFS/px4fmu_common/init.d/rc.octo | 94 -- ROMFS/px4fmu_common/init.d/rc.sensors | 23 +- ROMFS/px4fmu_common/init.d/rc.standalone | 13 - ROMFS/px4fmu_common/init.d/rc.usb | 33 - ROMFS/px4fmu_common/init.d/rcS | 785 +++++++------ ROMFS/px4fmu_test/init.d/rcS | 67 +- Tools/fsm_visualisation.py | 201 ++++ Tools/px4params/dokuwikiout.py | 30 +- Tools/px4params/dokuwikiout_listings.py | 27 + Tools/tests-host/Makefile | 10 +- Tools/tests-host/hrt.cpp | 16 + Tools/tests-host/mixer_test.cpp | 2 + Tools/tests-host/queue.h | 133 +++ makefiles/config_px4fmu-v1_backside.mk | 148 --- makefiles/config_px4fmu-v1_default.mk | 9 +- makefiles/config_px4fmu-v2_default.mk | 8 +- makefiles/config_px4fmu-v2_test.mk | 14 + nuttx-configs/px4fmu-v1/nsh/defconfig | 23 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 20 +- src/drivers/airspeed/airspeed.cpp | 2 +- src/drivers/boards/px4fmu-v1/board_config.h | 1 + src/drivers/boards/px4fmu-v2/board_config.h | 8 +- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 15 +- src/drivers/boards/px4io-v1/board_config.h | 6 +- src/drivers/boards/px4io-v2/board_config.h | 8 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 - src/drivers/drv_pwm_output.h | 4 + src/drivers/frsky_telemetry/frsky_data.c | 289 +++++ .../frsky_telemetry/frsky_data.h} | 19 +- src/drivers/frsky_telemetry/frsky_telemetry.c | 266 +++++ .../frsky_telemetry}/module.mk | 10 +- src/drivers/gps/gps.cpp | 220 ++-- src/drivers/hil/hil.cpp | 9 +- src/drivers/hmc5883/hmc5883.cpp | 160 +-- .../hott/hott_sensors/hott_sensors.cpp | 2 +- .../hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 26 +- src/drivers/ms5611/ms5611.cpp | 6 + src/drivers/px4fmu/fmu.cpp | 75 +- src/drivers/px4fmu/module.mk | 3 +- src/drivers/px4io/px4io.cpp | 223 ++-- src/drivers/px4io/px4io_uploader.cpp | 17 +- src/drivers/px4io/uploader.h | 2 +- src/drivers/rgbled/rgbled.cpp | 12 +- src/examples/fixedwing_control/main.c | 6 +- src/lib/conversion/rotation.cpp | 15 +- src/lib/conversion/rotation.h | 2 +- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- .../ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 43 +- src/lib/ecl/l1/ecl_l1_pos_controller.h | 12 +- src/lib/external_lgpl/tecs/tecs.cpp | 6 +- src/lib/external_lgpl/tecs/tecs.h | 6 +- src/lib/mathlib/math/Dcm.cpp | 174 --- src/lib/mathlib/math/Dcm.hpp | 108 -- src/lib/mathlib/math/EulerAngles.cpp | 126 -- src/lib/mathlib/math/Matrix.cpp | 193 --- src/lib/mathlib/math/Matrix.hpp | 416 ++++++- src/lib/mathlib/math/Quaternion.cpp | 174 --- src/lib/mathlib/math/Quaternion.hpp | 138 ++- src/lib/mathlib/math/Vector.cpp | 100 -- src/lib/mathlib/math/Vector.hpp | 477 +++++++- src/lib/mathlib/math/Vector2f.cpp | 103 -- src/lib/mathlib/math/Vector3.cpp | 99 -- src/lib/mathlib/math/arm/Matrix.hpp | 292 ----- src/lib/mathlib/math/arm/Vector.hpp | 236 ---- src/lib/mathlib/math/generic/Matrix.hpp | 437 ------- src/lib/mathlib/math/generic/Vector.hpp | 245 ---- src/lib/mathlib/mathlib.h | 8 +- src/lib/mathlib/module.mk | 17 - .../version/version.h} | 8 +- src/mainpage.dox | 9 + .../att_pos_estimator_ekf/KalmanNav.cpp | 124 +- .../att_pos_estimator_ekf/KalmanNav.hpp | 20 +- .../att_pos_estimator_ekf/kalman_main.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 115 +- .../attitude_estimator_ekf_params.c | 13 + .../attitude_estimator_ekf_params.h | 4 + .../commander/accelerometer_calibration.cpp | 14 +- src/modules/commander/commander.cpp | 415 ++++--- src/modules/commander/commander_helper.cpp | 44 +- src/modules/commander/commander_helper.h | 7 +- src/modules/commander/commander_params.c | 7 +- .../commander/state_machine_helper.cpp | 168 +-- src/modules/commander/state_machine_helper.h | 4 +- src/modules/controllib/uorb/blocks.cpp | 6 +- src/modules/controllib/uorb/blocks.hpp | 8 +- src/modules/fixedwing_backside/fixedwing.cpp | 8 +- src/modules/fixedwing_backside/fixedwing.hpp | 2 +- .../fixedwing_pos_control_main.c | 2 +- .../fw_att_control/fw_att_control_main.cpp | 10 +- .../fw_pos_control_l1_main.cpp | 144 ++- .../fw_pos_control_l1_params.c | 67 +- src/modules/mavlink/mavlink_orb_listener.cpp | 38 +- src/modules/mavlink/mavlink_receiver.cpp | 18 +- src/modules/mavlink/mavlink_receiver.h | 3 +- src/modules/mavlink_onboard/orb_topics.h | 2 +- .../mc_att_control/mc_att_control_main.cpp | 779 +++++++++++++ .../mc_att_control/mc_att_control_params.c} | 23 +- .../module.mk | 11 +- .../mc_pos_control/mc_pos_control_main.cpp | 1031 +++++++++++++++++ .../mc_pos_control/mc_pos_control_params.c} | 63 +- .../module.mk | 11 +- .../multirotor_att_control_main.c | 465 -------- .../multirotor_attitude_control.c | 254 ---- .../multirotor_attitude_control.h | 65 -- .../multirotor_rate_control.c | 196 ---- .../multirotor_rate_control.h | 64 - .../multirotor_pos_control.c | 553 --------- .../multirotor_pos_control_params.c | 112 -- .../multirotor_pos_control_params.h | 101 -- .../multirotor_pos_control/thrust_pid.c | 189 --- .../multirotor_pos_control/thrust_pid.h | 76 -- src/modules/navigator/navigator_main.cpp | 1011 +++++++++------- src/modules/navigator/navigator_params.c | 7 +- .../position_estimator_inav_main.c | 628 ++++++---- .../position_estimator_inav_params.c | 57 +- .../position_estimator_inav_params.h | 34 +- src/modules/px4iofirmware/adc.c | 53 +- src/modules/px4iofirmware/controls.c | 11 + src/modules/px4iofirmware/dsm.c | 6 + src/modules/px4iofirmware/mixer.cpp | 30 +- src/modules/px4iofirmware/px4io.c | 82 +- src/modules/px4iofirmware/px4io.h | 8 +- src/modules/px4iofirmware/registers.c | 26 +- src/modules/px4iofirmware/safety.c | 8 +- src/modules/px4iofirmware/sbus.c | 26 +- src/modules/sdlog2/sdlog2.c | 376 ++++-- src/modules/sdlog2/sdlog2_messages.h | 39 +- src/modules/sensors/module.mk | 2 +- src/modules/sensors/sensor_params.c | 250 +++- src/modules/sensors/sensors.cpp | 106 +- .../systemlib/board_serial.c} | 54 +- .../systemlib/board_serial.h} | 17 +- src/modules/systemlib/bson/tinybson.c | 3 + src/modules/systemlib/module.mk | 6 +- src/modules/systemlib/otp.c | 224 ++++ src/modules/systemlib/otp.h | 151 +++ src/modules/systemlib/param/param.c | 35 +- src/modules/systemlib/pid/pid.c | 106 +- src/modules/systemlib/pid/pid.h | 41 +- src/modules/systemlib/pwm_limit/pwm_limit.c | 110 +- src/modules/systemlib/pwm_limit/pwm_limit.h | 20 +- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/battery_status.h | 9 +- src/modules/uORB/topics/home_position.h | 2 +- ..._triplet.h => position_setpoint_triplet.h} | 41 +- src/modules/uORB/topics/vehicle_attitude.h | 1 + .../uORB/topics/vehicle_attitude_setpoint.h | 2 +- .../uORB/topics/vehicle_control_mode.h | 3 +- .../uORB/topics/vehicle_global_position.h | 17 +- .../uORB/topics/vehicle_local_position.h | 5 + src/modules/uORB/topics/vehicle_status.h | 16 +- src/systemcmds/eeprom/eeprom.c | 265 ----- .../Matrix.cpp => systemcmds/hw_ver/hw_ver.c} | 41 +- src/systemcmds/hw_ver/module.mk | 43 + src/systemcmds/{eeprom => mtd}/24xxxx_mtd.c | 0 src/systemcmds/mtd/module.mk | 6 + src/systemcmds/mtd/mtd.c | 493 ++++++++ src/systemcmds/nshterm/nshterm.c | 4 +- src/systemcmds/param/param.c | 47 +- src/systemcmds/ramtron/module.mk | 6 - src/systemcmds/ramtron/ramtron.c | 279 ----- src/systemcmds/tests/module.mk | 6 +- .../tests/test_conv.cpp} | 66 +- src/systemcmds/tests/test_file.c | 163 ++- src/systemcmds/tests/test_mathlib.cpp | 159 +++ src/systemcmds/tests/test_mixer.cpp | 197 +++- src/systemcmds/tests/test_mount.c | 289 +++++ src/systemcmds/tests/test_mtd.c | 229 ++++ src/systemcmds/tests/test_rc.c | 4 +- src/systemcmds/tests/tests.h | 6 +- src/systemcmds/tests/tests_main.c | 4 + 220 files changed, 10717 insertions(+), 9729 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil create mode 100644 ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/3031_io_phantom create mode 100644 ROMFS/px4fmu_common/init.d/3031_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/3033_io_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3033_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3034_fx79 delete mode 100644 ROMFS/px4fmu_common/init.d/3034_io_fx79 create mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 delete mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/cmp_test create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.hexa create mode 100644 ROMFS/px4fmu_common/init.d/rc.interface create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor delete mode 100644 ROMFS/px4fmu_common/init.d/rc.octo delete mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone create mode 100755 Tools/fsm_visualisation.py create mode 100644 Tools/px4params/dokuwikiout_listings.py create mode 100644 Tools/tests-host/hrt.cpp create mode 100644 Tools/tests-host/queue.h delete mode 100644 makefiles/config_px4fmu-v1_backside.mk create mode 100644 src/drivers/frsky_telemetry/frsky_data.c rename src/{lib/mathlib/math/generic/Vector.cpp => drivers/frsky_telemetry/frsky_data.h} (79%) create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c rename src/{systemcmds/eeprom => drivers/frsky_telemetry}/module.mk (89%) delete mode 100644 src/lib/mathlib/math/Dcm.cpp delete mode 100644 src/lib/mathlib/math/Dcm.hpp delete mode 100644 src/lib/mathlib/math/EulerAngles.cpp delete mode 100644 src/lib/mathlib/math/Matrix.cpp delete mode 100644 src/lib/mathlib/math/Quaternion.cpp delete mode 100644 src/lib/mathlib/math/Vector.cpp delete mode 100644 src/lib/mathlib/math/Vector2f.cpp delete mode 100644 src/lib/mathlib/math/Vector3.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.hpp delete mode 100644 src/lib/mathlib/math/arm/Vector.hpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.hpp delete mode 100644 src/lib/mathlib/math/generic/Vector.hpp rename src/{modules/sdlog2/sdlog2_version.h => lib/version/version.h} (95%) create mode 100644 src/mainpage.dox create mode 100644 src/modules/mc_att_control/mc_att_control_main.cpp rename src/{lib/mathlib/math/arm/Vector.cpp => modules/mc_att_control/mc_att_control_params.c} (68%) rename src/modules/{multirotor_pos_control => mc_att_control}/module.mk (87%) create mode 100644 src/modules/mc_pos_control/mc_pos_control_main.cpp rename src/{lib/mathlib/math/Vector2f.hpp => modules/mc_pos_control/mc_pos_control_params.c} (67%) rename src/modules/{multirotor_att_control => mc_pos_control}/module.mk (86%) mode change 100755 => 100644 delete mode 100644 src/modules/multirotor_att_control/multirotor_att_control_main.c delete mode 100644 src/modules/multirotor_att_control/multirotor_attitude_control.c delete mode 100644 src/modules/multirotor_att_control/multirotor_attitude_control.h delete mode 100644 src/modules/multirotor_att_control/multirotor_rate_control.c delete mode 100644 src/modules/multirotor_att_control/multirotor_rate_control.h delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control.c delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control_params.c delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control_params.h delete mode 100644 src/modules/multirotor_pos_control/thrust_pid.c delete mode 100644 src/modules/multirotor_pos_control/thrust_pid.h rename src/{lib/mathlib/math/Vector3.hpp => modules/systemlib/board_serial.c} (65%) rename src/{lib/mathlib/math/arm/Matrix.cpp => modules/systemlib/board_serial.h} (84%) create mode 100644 src/modules/systemlib/otp.c create mode 100644 src/modules/systemlib/otp.h rename src/modules/uORB/topics/{mission_item_triplet.h => position_setpoint_triplet.h} (72%) delete mode 100644 src/systemcmds/eeprom/eeprom.c rename src/{lib/mathlib/math/generic/Matrix.cpp => systemcmds/hw_ver/hw_ver.c} (69%) create mode 100644 src/systemcmds/hw_ver/module.mk rename src/systemcmds/{eeprom => mtd}/24xxxx_mtd.c (100%) create mode 100644 src/systemcmds/mtd/module.mk create mode 100644 src/systemcmds/mtd/mtd.c delete mode 100644 src/systemcmds/ramtron/module.mk delete mode 100644 src/systemcmds/ramtron/ramtron.c rename src/{lib/mathlib/math/EulerAngles.hpp => systemcmds/tests/test_conv.cpp} (66%) create mode 100644 src/systemcmds/tests/test_mathlib.cpp create mode 100644 src/systemcmds/tests/test_mount.c create mode 100644 src/systemcmds/tests/test_mtd.c diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 2417028114..b674fbc485 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -599,7 +599,7 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../src/modules/mathlib/CMSIS \ +EXCLUDE = ../src/lib/mathlib/CMSIS \ ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 40a13b5d17..ebe8a1a1ea 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 81d4b5d570..56c74a3b52 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,29 @@ #!nsh - -echo "[init] Team Blacksheep Discovery Quad" - # -# Load default params for this platform +# Team Blacksheep Discovery Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 param set MC_YAWRATE_P 0.2 - - param save + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 +set VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79e..a3bcb63eb8 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,49 @@ #!nsh - -echo "[init] 3DR Iris Quad" - # -# Load default params for this platform +# 3DR Iris Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 - param save + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 +set VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.0098 - set EXIT_ON_END yes -fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil deleted file mode 100644 index 9b664d63ed..0000000000 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ /dev/null @@ -1,105 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] HILS quadrotor starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil new file mode 100644 index 0000000000..2d497374a5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -0,0 +1,42 @@ +#!nsh +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 +fi + +set HIL yes + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6d..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad343..e958448912 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,10 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + +# +# Maintainers: Anton Babushkin # -echo "[HIL] HILS quadrotor + starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" +sh /etc/init.d/1001_rc_quad_x.hil +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 344d784224..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,59 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -set MODE autostart -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - - -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 0000000000..5f3cec4e0a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_cox + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c903..0e5bf60d60 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,15 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" - # -# Load default params for this platform +# MPX EasyStar Plane # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: ??? +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +33,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba074..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index a6d2ace962..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 65f01c9743..cbcc6189b6 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom deleted file mode 100644 index 0cf6ee39a5..0000000000 --- a/ROMFS/px4fmu_common/init.d/3031_io_phantom +++ /dev/null @@ -1,85 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom new file mode 100644 index 0000000000..4ebbe9c61e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -0,0 +1,44 @@ +#!nsh +# +# Phantom FPV Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 41e0416548..143310af95 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,58 +1,43 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" - # -# Load default params for this platform +# Skywalker X5 Flying Wing # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Thomas Gubler , Julian Oes +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing deleted file mode 100644 index 82ff425e6d..0000000000 --- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing new file mode 100644 index 0000000000..e537632784 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -0,0 +1,43 @@ +#!nsh +# +# Wing Wing (aka Z-84) Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 new file mode 100644 index 0000000000..8d179d1fd4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -0,0 +1,43 @@ +#!nsh +# +# FX-79 Buffalo Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 deleted file mode 100644 index 759c17bb44..0000000000 --- a/ROMFS/px4fmu_common/init.d/3034_io_fx79 +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow index 2886bcb752..e2cb8833dc 100644 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow @@ -67,7 +67,7 @@ flow_position_estimator start # # Fire up the multi rotor attitude controller # -multirotor_att_control start +mc_att_control_vector start # # Fire up the flow position controller diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e24..e0cf92d97f 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,69 +1,47 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - # -# Load default params for this platform +# DJI Flame Wheel F330 Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 +set VEHICLE_TYPE mc +set MIXER FMU_quad_x - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.mc_interface - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191d..ced69783db 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,74 +1,35 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - # -# Load default params for this platform +# DJI Flame Wheel F450 Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.3 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +set VEHICLE_TYPE mc +set MIXER FMU_quad_x -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 0000000000..e1423e0082 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,31 @@ +#!nsh +# +# HobbyKing X550 Quadcopter +# +# Maintainers: Todd Stellanova +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_YAW_P 0.6 + param set MC_YAW_I 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b4403..0000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 0000000000..2e5f6ca4f5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 0000000000..ddec8f36ef --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb5..0000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 0000000000..106e0fb54c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 0000000000..f0eea339b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 9b90cbdd04..0000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param save -fi - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if [ $BOARD == fmuv1 ] - then - sdlog2 start -r 50 -e -b 16 - else - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 0000000000..992a7aeba6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85bb..0000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 0000000000..34da2dfef2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,195 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +# AUTOSTART PARTITION: +# 0 .. 999 Reserved (historical) +# 1000 .. 1999 Simulation setups +# 2000 .. 2999 Standard planes +# 3000 .. 3999 Flying wing +# 4000 .. 4999 Quad X +# 5000 .. 5999 Quad + +# 6000 .. 6999 Hexa X +# 7000 .. 7999 Hexa + +# 8000 .. 8999 Octo X +# 9000 .. 9999 Octo + +# 10000 .. 10999 Wide arm / H frame +# 11000 .. 11999 Hexa Cox +# 12000 .. 12999 Octo Cox + +# +# Simulation setups +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad_x.hil +fi + +if param compare SYS_AUTOSTART 1002 +then + sh /etc/init.d/1002_rc_fw_state.hil +fi + +if param compare SYS_AUTOSTART 1003 +then + sh /etc/init.d/1003_rc_quad_+.hil +fi + +if param compare SYS_AUTOSTART 1004 +then + sh /etc/init.d/1004_rc_fw_Rascal110.hil +fi + +# +# Standard plane +# + +if param compare SYS_AUTOSTART 2100 100 +then + sh /etc/init.d/2100_mpx_easystar + set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + sh /etc/init.d/2101_hk_bixler + set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + sh /etc/init.d/2102_3dr_skywalker + set MODE custom +fi + +# +# Flying wing +# + +if param compare SYS_AUTOSTART 3030 30 +then + sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + sh /etc/init.d/3031_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + sh /etc/init.d/3033_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + sh /etc/init.d/3034_fx79 +fi + +# +# Quad X +# + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +fi + +if param compare SYS_AUTOSTART 4010 10 +then + sh /etc/init.d/4010_dji_f330 +fi + +if param compare SYS_AUTOSTART 4011 11 +then + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 12 +then + sh /etc/init.d/4012_hk_x550 +fi + +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 15 +then + sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 16 +then + sh /etc/init.d/10016_3dr_iris +fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68bd..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e529..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f028510064..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 0000000000..d354fb06fa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e48..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 0000000000..d25f01dde0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined" + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + echo "[init] Set PWM disarmed: $PWM_DISARMED" + pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + echo "[init] Set PWM min: $PWM_MIN" + pwm min -c $PWM_OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + echo "[init] Set PWM max: $PWM_MAX" + pwm max -c $PWM_OUTPUTS -p $PWM_MAX + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index aaf91b3166..c9d964f8e2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,21 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -if px4io start -then - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - # - # Disable px4io topic limiting - # - if [ $BOARD == fmuv1 ] - then - px4io limit 200 - else - px4io limit 400 - fi -else - # SOS - tone_alarm error +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust PX4IO update rate limit +# +set PX4IO_LIMIT 400 +if hw_ver compare PX4FMU_V1 +then + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055a..dcf5bbced8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -1,14 +1,16 @@ #!nsh # -# Initialise logging services. +# Initialize logging services. # if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then - sdlog2 start -r 50 -a -b 16 + echo "Start sdlog2 at 50Hz" + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + echo "Start sdlog2 at 200Hz" + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps new file mode 100644 index 0000000000..96fe32c8a7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -0,0 +1,24 @@ +#!nsh +# +# Standard apps for multirotors +# + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +mc_att_control start + +# +# Start position control +# +mc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6bb2e84ecf..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,49 +0,0 @@ -#!nsh -# -# Script to set PWM min / max limits and mixer -# - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -if [ $FRAME_COUNT == 4 ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then - set OUTPUTS 123456 - param set MAV_TYPE 13 - else - set OUTPUTS 12345678 - fi -fi - - -# -# Set PWM output frequency -# -pwm rate -c $OUTPUTS -r $PWM_RATE - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor deleted file mode 100644 index bc550ac5a1..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ /dev/null @@ -1,39 +0,0 @@ -#!nsh -# -# Standard everything needed for multirotors except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e5..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 070a4e7e35..badbf92c3c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,41 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - set BOARD fmuv2 -else - set BOARD fmuv1 + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b9..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47ea..0cd8a0e042 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# Start px4io if present -if px4io start -then - echo "PX4IO driver started" -else - if fmu mode_serial - then - echo "FMU driver started" - fi -fi - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status -then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status - then - echo "fixedwing att filter running" - else - attitude_estimator_ekf start - fi -fi - -# Start GPS -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 90f2e2b178..6f4e1f3b56 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,39 +8,40 @@ # set MODE autostart -set logfile /fs/microsd/bootlog.txt +set RC_FILE /fs/microsd/etc/rc.txt +set CONFIG_FILE /fs/microsd/etc/config.txt +set EXTRAS_FILE /fs/microsd/etc/extras.txt + +set TUNE_OUT_ERROR ML<> $LOG_FILE + + set IO_PRESENT yes + else + echo "[init] Trying to update" + echo "PX4IO Trying to update" >> $LOG_FILE + + tone_alarm MLL32CP8MB + + if px4io forceupdate 14662 $IO_FILE + then + usleep 500000 + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK, update successful" + echo "PX4IO CRC OK after updating" >> $LOG_FILE + tone_alarm MLL8CDE + + set IO_PRESENT yes + else + echo "[init] ERROR: PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR + fi + else + echo "[init] ERROR: PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR + fi + fi + + if [ $IO_PRESENT == no ] + then + echo "[init] ERROR: PX4IO not found" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] + then + if [ $USE_IO == yes ] + then + set OUTPUT_MODE io + else + set OUTPUT_MODE fmu + fi + fi + + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] + then + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + fi + + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + # # Start the Commander (needs to be this early for in-air-restarts) # commander start - - # - # Start the datamanager - # - dataman start - - # - # Start the Navigator - # - navigator start - - if param compare SYS_AUTOSTART 1000 - then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom - fi - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1003 - then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1004 - then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom - fi - - if [ $MODE != custom ] - then - # Try to get an USB console - nshterm /dev/ttyACM0 & - fi - # - # Upgrade PX4IO firmware + # Start primary output # - - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set io_file /etc/extras/px4io-v2_default.bin - else - set io_file /etc/extras/px4io-v1_default.bin - fi - - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi + set TTYS1_BUSY no - if px4io checkcrc $io_file + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output + if [ $OUTPUT_MODE != none ] then - echo "PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile - else - echo "PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile - tone_alarm MBABGP - if px4io forceupdate 14662 $io_file + if [ $OUTPUT_MODE == io ] then - usleep 500000 + echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile - tone_alarm MSPAA + echo "[init] PX4IO started" + sh /etc/init.d/rc.io else - echo "PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + if [ $OUTPUT_MODE == fmu ] + then + echo "[init] Use FMU PWM as primary output" + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG + then + echo "[init] MKBLCTRL started" + else + echo "[init] ERROR: MKBLCTRL start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] ERROR: HIL output start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Start IO or FMU for RC PPM input if needed + # + if [ $IO_PRESENT == yes ] + then + if [ $OUTPUT_MODE != io ] + then + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi fi else - echo "PX4IO update failed" - echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi fi fi - + + # + # MAVLink + # set EXIT_ON_END no - # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) - # - # AUTOSTART PARTITION: - # 0 .. 999 Reserved (historical) - # 1000 .. 1999 Simulation setups - # 2000 .. 2999 Standard planes - # 3000 .. 3999 Flying wing - # 4000 .. 4999 Quad X - # 5000 .. 5999 Quad + - # 6000 .. 6999 Hexa X - # 7000 .. 7999 Hexa + - # 8000 .. 8999 Octo X - # 9000 .. 9999 Octo + - # 10000 .. 10999 Wide arm / H frame - # 11000 .. 11999 Hexa Cox - # 12000 .. 12999 Octo Cox - - if param compare SYS_AUTOSTART 4008 8 + if [ $HIL == yes ] then - sh /etc/init.d/4008_ardrone - set MODE custom - fi - - if param compare SYS_AUTOSTART 4009 9 - then - sh /etc/init.d/4009_ardrone_flow - set MODE custom - fi - - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 12001 - then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 - then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi - - if param compare SYS_AUTOSTART 3031 31 - then - sh /etc/init.d/3031_io_phantom - set MODE custom - fi - - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi - - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom - fi - - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - - if param compare SYS_AUTOSTART 2100 100 - then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi - - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom - fi - - if param compare SYS_AUTOSTART 2102 102 - then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom - fi - - if param compare SYS_AUTOSTART 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing - if [ -f /fs/microsd/etc/rc.local ] - then - sh /fs/microsd/etc/rc.local - fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] - then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect + sleep 1 + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $TTYS1_BUSY == yes ] then - px4io start + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes else - if fmu mode_serial + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi + fi + + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + + echo "[init] Start GPS" + gps start + fi + + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] + then + echo "[init] Vehicle type: FIXED WING" + + if [ $MIXER == none ] + then + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.fw_apps + fi + + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] + then + echo "[init] Vehicle type: MULTICOPTER" + + if [ $MIXER == none ] + then + # Set default mixer for multicopter if not defined + set MIXER quad_x + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] then - echo "FMU driver (no PWM) started" + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 fi fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - + + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps + fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + fi + + # Start any custom addons + if [ -f $EXTRAS_FILE ] + then + echo "[init] Starting addons script: $EXTRAS_FILE" + sh $EXTRAS_FILE + else + echo "[init] Addons script not found: $EXTRAS_FILE" fi if [ $EXIT_ON_END == yes ] diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6aa1d3d46b..56482d1407 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,6 +2,7 @@ # # PX4FMU startup script for test hackery. # +uorb start if sercon then @@ -9,4 +10,68 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & -fi \ No newline at end of file +fi + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +# +# Start a minimal system +# + +if [ -f /etc/extras/px4io-v2_default.bin ] +then + set io_file /etc/extras/px4io-v2_default.bin +else + set io_file /etc/extras/px4io-v1_default.bin +fi + +if px4io start +then + echo "PX4IO OK" +fi + +if px4io checkcrc $io_file +then + echo "PX4IO CRC OK" +else + echo "PX4IO CRC failure" + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file + then + usleep 500000 + if px4io start + then + echo "PX4IO restart OK" + tone_alarm MSPAA + else + echo "PX4IO restart failed" + tone_alarm MNGGG + sleep 5 + reboot + fi + else + echo "PX4IO update failed" + tone_alarm MNGGG + fi +fi + +# +# The presence of this file suggests we're running a mount stress test +# +if [ -f /fs/microsd/mount_test_cmds.txt ] +then + tests mount +fi diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py new file mode 100755 index 0000000000..c678ef0f4b --- /dev/null +++ b/Tools/fsm_visualisation.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 + +"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table + +convert dot code to png using graphviz: + +dot fsm.dot -Tpng -o fsm.png +""" + +import argparse +import re + +__author__ = "Julian Oes" + +def get_dot_header(): + + return """digraph finite_state_machine { + graph [ dpi = 300 ]; + ratio = 1.5 + node [shape = circle];""" + +def get_dot_footer(): + + return """}\n""" + +def main(): + + # parse input arguments + parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.') + parser.add_argument("-i", "--input-file", default=None, help="choose file to parse") + parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table") + args = parser.parse_args() + + # open source file + if args.input_file == None: + exit('please specify file') + f = open(args.input_file,'r') + source = f.read() + + # search for state transition table and extract the table itself + # first look for StateTable::Tran + # then accept anything including newline until { + # but don't accept the definition (without ;) + # then extract anything inside the brackets until }; + match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source) + + if not match: + exit('no state transition table found') + + table_source = match.group(1) + + # bookkeeping for error checking + num_errors_found = 0 + + states = [] + events = [] + + # first get all states and events + for table_line in table_source.split('\n'): + + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) + if match: + states.append(str(match.group(1))) + # go to next line + continue + + if len(states) == 1: + match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line) + if match: + events.append(str(match.group(1))) + + print('Found %d states and %d events' % (len(states), len(events))) + + + # keep track of origin state + state = None + + # fill dot code in here + dot_code = '' + + # create table len(states)xlen(events) + transition_table = [[[] for x in range(len(states))] for y in range(len(events))] + + # now fill the transition table and write the dot code + for table_line in table_source.split('\n'): + + # get states + # from: /* NAV_STATE_NONE */ + # extract only "NONE" + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) + if match: + state = match.group(1) + state_index = states.index(state) + # go to next line + continue + + # can't advance without proper state + if state == None: + continue + + # get event and next state + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY} + # extract "READY_REQUESTED" and "READY" if there is ACTION + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line) + + # get event and next state + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + # extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line) + + # ignore lines with brackets only + if match_action or match_no_action: + + # only write arrows for actions + if match_action: + event = match_action.group(1) + new_state = match_action.group(2) + dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n' + + elif match_no_action: + event = match_no_action.group(1) + new_state = match_no_action.group(2) + + # check for state changes without action + if state != new_state: + print('Error: no action but state change:') + print('State: ' + state + ' changed to: ' + new_state) + print(table_line) + num_errors_found += 1 + + # check for wrong events + if event not in events: + print('Error: unknown event: ' + event) + print(table_line) + num_errors_found += 1 + + # check for wrong new states + if new_state not in states: + print('Error: unknown new state: ' + new_state) + print(table_line) + num_errors_found += 1 + + # save new state in transition table + event_index = events.index(event) + + # bold for action + if match_action: + transition_table[event_index][state_index] = '**' + new_state + '**' + else: + transition_table[event_index][state_index] = new_state + + + + # assemble dot code + dot_code = get_dot_header() + dot_code + get_dot_footer() + + # write or print dot file + if args.dot_file: + f = open(args.dot_file,'w') + f.write(dot_code) + print('Wrote dot file') + else: + print('##########Dot-start##########') + print(dot_code) + print('##########Dot-end############') + + + # assemble doku wiki table + table_code = '| ^ ' + # start with header of all states + for state in states: + table_code += state + ' ^ ' + + table_code += '\n' + + # add events and new states + for event, row in zip(events, transition_table): + table_code += '^ ' + event + ' | ' + for new_state in row: + table_code += new_state + ' | ' + table_code += '\n' + + # write or print wiki table + if args.table_file: + f = open(args.table_file,'w') + f.write(table_code) + print('Wrote table file') + else: + print('##########Table-start########') + print(table_code) + print('##########Table-end##########') + + # report obvous errors + if num_errors_found: + print('Obvious errors found: %d' % num_errors_found) + else: + print('No obvious errors found') + +if __name__ == '__main__': + main() diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 33f76b415e..4d40a6201d 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output): result = "" for group in groups: result += "==== %s ====\n\n" % group.GetName() + result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") name = param.GetFieldValue("short_desc") - if code != name: - name = "%s (%s)" % (name, code) - result += "=== %s ===\n\n" % name - long_desc = param.GetFieldValue("long_desc") - if long_desc is not None: - result += "%s\n\n" % long_desc + name = name.replace("\n", "") + result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") if min_val is not None: - result += "* Minimal value: %s\n" % min_val + result += "| %s " % min_val + else: + result += "|" max_val = param.GetFieldValue("max") if max_val is not None: - result += "* Maximal value: %s\n" % max_val + result += "| %s " % max_val + else: + result += "|" def_val = param.GetFieldValue("default") if def_val is not None: - result += "* Default value: %s\n" % def_val - result += "\n" + result += "| %s " % def_val + else: + result += "|" + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + long_desc = long_desc.replace("\n", "") + result += "| %s " % long_desc + else: + result += "|" + result += "|\n" + result += "\n" return result diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py new file mode 100644 index 0000000000..33f76b415e --- /dev/null +++ b/Tools/px4params/dokuwikiout_listings.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 97410ff47c..7ab1454f01 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,11 +10,13 @@ LIBS=-lm #_DEPS = test.h #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \ + mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) #$(DEPS) $(ODIR)/%.o: %.cpp + mkdir -p obj $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp @@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(CC) -c -o $@ $< $(CFLAGS) +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c + $(CC) -c -o $@ $< $(CFLAGS) + $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c $(CC) -c -o $@ $< $(CFLAGS) diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp new file mode 100644 index 0000000000..01b5958b7c --- /dev/null +++ b/Tools/tests-host/hrt.cpp @@ -0,0 +1,16 @@ +#include +#include +#include +#include + +hrt_abstime hrt_absolute_time() { + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; +} + +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { + // not thread safe + return hrt_absolute_time() - *then; +} diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 042322aadc..e311617f96 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -9,4 +9,6 @@ int main(int argc, char *argv[]) { "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; test_mixer(3, args); + + test_conv(1, args); } \ No newline at end of file diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h new file mode 100644 index 0000000000..0fdb170db6 --- /dev/null +++ b/Tools/tests-host/queue.h @@ -0,0 +1,133 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +#ifndef FAR +#define FAR +#endif + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index 6d2a9f7bd4..0000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,148 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/blinkm -MODULES += drivers/rgbled -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/att_pos_estimator_ekf - -# -# Vehicle Control -# -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB -MODULES += modules/dataman - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion -MODULES += lib/launchdetection - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index fba50aaf81..51be7e1a12 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -36,13 +36,13 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # # System commands # -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron +MODULES += systemcmds/mtd MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c @@ -57,6 +57,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control @@ -81,8 +82,8 @@ MODULES += modules/position_estimator_inav # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control #MODULES += examples/flow_position_control #MODULES += examples/flow_speed_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index f0a9c0c065..ab05d4e3da 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -36,6 +36,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # Needs to be burned to the ground and re-written; for now, @@ -45,7 +46,6 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/ramtron MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer @@ -59,6 +59,8 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/hw_ver # # General system control @@ -83,8 +85,8 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control # # Logging diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0f60e88b5e..8623c0584b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -6,23 +6,37 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules # MODULES += drivers/device MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/px4io MODULES += systemcmds/perf MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/hw_ver # # Library modules # MODULES += modules/systemlib +MODULES += modules/systemlib/mixer MODULES += modules/uORB +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS + # # Transitional support - add commands from the NuttX export archive. # diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e8..1dc96b3c3f 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1 CONFIG_MMCSD_SPI=y CONFIG_MMCSD_SPICLOCK=24000000 # CONFIG_MMCSD_SDIO is not set -# CONFIG_MTD is not set +CONFIG_MTD=y CONFIG_PIPES=y # CONFIG_PM is not set # CONFIG_POWER is not set @@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +# CONFIG_MTD_RAMTRON is not set +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set + # # USART1 Configuration # @@ -566,7 +585,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb3637..2a734c27e5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -295,16 +295,16 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set CONFIG_USART3_RXDMA=y # CONFIG_UART4_RS485 is not set CONFIG_UART4_RXDMA=y -# CONFIG_UART5_RXDMA is not set +CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set @@ -500,8 +500,8 @@ CONFIG_MTD=y # # MTD Configuration # -# CONFIG_MTD_PARTITION is not set -# CONFIG_MTD_BYTE_WRITE is not set +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y # # MTD Device Drivers @@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936c..f73a3ef01e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 6f71662846..02c26b5c0b 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -60,6 +60,7 @@ __BEGIN_DECLS /* PX4IO connection configuration */ #define PX4IO_SERIAL_DEVICE "/dev/ttyS2" +#define UDID_START 0x1FFF7A10 //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index a19ed9d247..7cfca7656e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -52,6 +52,8 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include #include + +#define UDID_START 0x1FFF7A10 /**************************************************************************************************** * Definitions @@ -73,7 +75,7 @@ __BEGIN_DECLS /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) @@ -85,7 +87,7 @@ __BEGIN_DECLS #define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) #define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) #define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) -#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI1 off */ #define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) @@ -96,7 +98,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 269ec238eb..71414d62c7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\n"); + message("[boot] Initialized SPI port 1 (SENSORS)\n"); /* Get the SPI port for the FRAM */ @@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 375000000); + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - message("[boot] Successfully initialized SPI port 2\n"); + message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } @@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { - message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index c3f39addfe..1be4877bad 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -58,11 +58,11 @@ /* PX4IO GPIOs **********************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8da5552110..ef9bb5cadd 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -74,9 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) /* Safety switch button *******************************************************/ @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf56..9f8c0eeb27 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37b..88da94b1ed 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 0000000000..e201ecbb3d --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 frsky_data.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +/* FrSky sensor hub data IDs */ +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +#define frac(f) (f - (int)f) + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; +static int vehicle_status_sub = -1; + +/** + * Initializes the uORB subscriptions. + */ +void frsky_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * Sends a 0x5E start/stop byte. + */ +static void frsky_send_startstop(int uart) +{ + static const uint8_t c = 0x5E; + write(uart, &c, sizeof(c)); +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void frsky_send_byte(int uart, uint8_t value) +{ + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; + + switch (value) { + case 0x5E: + write(uart, x5E, sizeof(x5E)); + break; + + case 0x5D: + write(uart, x5D, sizeof(x5D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +static void frsky_send_data(int uart, uint8_t id, int16_t data) +{ + /* Cast data to unsigned, because signed shift might behave incorrectly */ + uint16_t udata = data; + + frsky_send_startstop(uart); + + frsky_send_byte(uart, id); + frsky_send_byte(uart, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, barometer altitude, temperature, battery voltage & current + */ +void frsky_send_frame1(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + frsky_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter) * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); + + frsky_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + frsky_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); + + frsky_send_startstop(uart); +} + +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +static float frsky_format_gps(float dec) +{ + float dms_deg = (int) dec; + float dec_deg = dec - dms_deg; + float dms_min = (int) (dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + +/** + * Sends frame 2 (every 1000ms): + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level + */ +void frsky_send_frame2(int uart) +{ + /* get a local copy of the global position data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send formatted frame */ + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; + char lat_ns = 0, lon_ew = 0; + int sec = 0; + if (global_pos.valid) { + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + lat = frsky_format_gps(abs(global_pos.lat)); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = frsky_format_gps(abs(global_pos.lon)); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) + * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; + } + + frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); + + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); + + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * GPS date & time + */ +void frsky_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); + + frsky_send_startstop(uart); +} diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/drivers/frsky_telemetry/frsky_data.h similarity index 79% rename from src/lib/mathlib/math/generic/Vector.cpp rename to src/drivers/frsky_telemetry/frsky_data.h index 7ea6496bb1..a7d9eee533 100644 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +33,19 @@ ****************************************************************************/ /** - * @file Vector.cpp + * @file frsky_data.h + * @author Stefan Rado + * + * FrSky telemetry implementation. * - * math vector */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H -#include "Vector.hpp" +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); + +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c new file mode 100644 index 0000000000..7b08ca69e9 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 frsky_telemetry.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + 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 */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + static const speed_t speed = B9600; + + 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; +} + +/** + * Print command usage information + */ +static void usage() +{ + fprintf(stderr, + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* Print welcome text */ + warnx("FrSky telemetry interface starting..."); + + /* Open UART */ + struct termios uart_config_original; + const int uart = frsky_open_uart(device_name, &uart_config_original); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) + { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) + { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int frsky_telemetry_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, "frsky_telemetry already running"); + + thread_should_exit = false; + frsky_task = task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + frsky_telemetry_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, "frsky_telemetry 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/systemcmds/eeprom/module.mk b/src/drivers/frsky_telemetry/module.mk similarity index 89% rename from src/systemcmds/eeprom/module.mk rename to src/drivers/frsky_telemetry/module.mk index 07f3945be2..1632c74f75 100644 --- a/src/systemcmds/eeprom/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-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 @@ -32,8 +32,10 @@ ############################################################################ # -# EEPROM file system driver +# FrSky telemetry application. # -MODULE_COMMAND = eeprom -SRCS = 24xxxx_mtd.c eeprom.c +MODULE_COMMAND = frsky_telemetry + +SRCS = frsky_data.c \ + frsky_telemetry.c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ecd..6b72d560fa 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,98 +266,133 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } + if (_fake_gps) { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + //no time and satellite information simulated - default: - break; - } + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - unlock(); - - if (_Helper->configure(_baudrate) == 0) { - unlock(); - - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); - - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } - - if (!_healthy) { - char *mode_str = "unknown"; - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; - - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; - - default: - break; - } - - warnx("module found: %s", mode_str); - _healthy = true; - } + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + usleep(2e5); + + } else { + + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } + + unlock(); + + if (_Helper->configure(_baudrate) == 0) { + unlock(); + + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); + + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } + + if (!_healthy) { + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; + } + } + + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } + + lock(); } lock(); - } - lock(); + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - - default: - break; + default: + break; + } } } @@ -417,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -427,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -435,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -527,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -542,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -567,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87f..0a047f38f1 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -193,9 +193,10 @@ HIL::~HIL() } while (_task != -1); } - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // XXX already claimed with CDEV + // /* clean up the alternate device node */ + // if (_primary_pwm_device) + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); g_hil = nullptr; } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66e..9b9c11af29 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,32 +915,17 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; + + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; warnx("starting mag scale calibration"); - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { warn("failed to set 2Hz poll rate"); @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* Set to 2.5 Gauss. We ask for 3 to get the right part of + * the chained if statement above. */ + if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("failed to set 2.5 Ga range"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; } //warnx("periodic read %u", i); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#if 0 + warnx("measurement avg: %.6f %.6f %.6f", + (double)sum_excited[0]/good_count, + (double)sum_excited[1]/good_count, + (double)sum_excited[2]/good_count); +#endif float scaling[3]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index e322c6349e..a3d3a39333 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 042d9f8162..d293f9954e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d67203..9251cff7b2 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include @@ -178,31 +177,26 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_press_pa; } @@ -390,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a9..6326cf7fcb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b878d29bcc..c067d363b7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include @@ -224,10 +225,10 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), - _failsafe_pwm( {0}), - _disarmed_pwm( {0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _failsafe_pwm({0}), + _disarmed_pwm({0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -575,7 +576,7 @@ PX4FMU::task_main() if (i >= outputs.noutputs || !isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + outputs.output[i] > 1.0f) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(3): case PWM_SERVO_SET(2): if (_mode < MODE_4PWM) { @@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): if (arg <= 2100) { @@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(3): case PWM_SERVO_GET(2): if (_mode < MODE_4PWM) { @@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); @@ -1005,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1107,10 +1142,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_OFF); stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF); @@ -1123,10 +1160,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); /* set the sensor rail off */ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); @@ -1159,6 +1198,13 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1431,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -1591,6 +1636,15 @@ fmu_main(int argc, char *argv[]) errx(0, "FMU driver stopped"); } + if (!strcmp(verb, "id")) { + char id[12]; + (void)get_board_serial(id); + + errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", + (unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5], + (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]); + } + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); @@ -1647,6 +1701,7 @@ fmu_main(int argc, char *argv[]) sensor_reset(0); warnx("resettet default time"); } + exit(0); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index d918abd572..05bc7a5b33 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,5 +3,4 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp \ - ../../modules/systemlib/pwm_limit/pwm_limit.c +SRCS = fmu.cpp diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cbdd5acc44..df847a64da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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 @@ -35,7 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via I2C. + * PX4IO is connected via I2C or DMA enabled high-speed UART. */ #include @@ -270,7 +270,8 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; /// 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1450,6 +1454,13 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } + /* set RSSI */ + + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } + /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); @@ -1664,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1688,7 +1710,21 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } + + if (ret) + return ret; retries--; @@ -1798,7 +1834,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2355,8 +2391,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2419,6 +2457,69 @@ detect(int argc, char *argv[]) } } +void +checkcrc(int argc, char *argv[]) +{ + bool keep_running = false; + + if (g_dev == nullptr) { + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + } else { + /* its already running, don't kill the driver */ + keep_running = true; + } + + /* + check IO CRC against CRC of a file + */ + if (argc < 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + int fd = open(argv[1], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[1], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + + if (!keep_running) { + delete g_dev; + g_dev = nullptr; + } + + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); +} + void bind(int argc, char *argv[]) { @@ -2569,17 +2670,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); @@ -2613,12 +2714,16 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "detect")) detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "checkcrc")) + checkcrc(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2691,18 +2796,30 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } - // tear down the px4io instance - delete g_dev; + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); + } } + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + g_dev = nullptr; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; @@ -2760,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } @@ -2798,49 +2916,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "checkcrc")) { - /* - check IO CRC against CRC of a file - */ - if (argc <= 2) { - printf("usage: px4io checkcrc filename\n"); - exit(1); - } - if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - int fd = open(argv[2], O_RDONLY); - if (fd == -1) { - printf("open of %s failed - %d\n", argv[2], errno); - exit(1); - } - const uint32_t app_size_max = 0xf000; - uint32_t fw_crc = 0; - uint32_t nbytes = 0; - while (true) { - uint8_t buf[16]; - int n = read(fd, buf, sizeof(buf)); - if (n <= 0) break; - fw_crc = crc32part(buf, n, fw_crc); - nbytes += n; - } - close(fd); - while (nbytes < app_size_max) { - uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); - nbytes++; - } - - int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); - if (ret != OK) { - printf("check CRC failed - %d\n", ret); - exit(1); - } - printf("CRCs match\n"); - exit(0); - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee1..dd8abbac5a 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ @@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e23..55f63eef96 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 727c86e02a..4f58891ed3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { warnx("not started"); rgbled_usage(); - exit(0); + exit(1); } if (!strcmp(verb, "test")) { @@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { @@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[]) exit(ret); } + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } + if (!strcmp(verb, "rgb")) { if (argc < 5) { errx(1, "Usage: rgbled rgb "); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e00077..067d773649 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b078562c2d..614877b183 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,22 +41,11 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) { - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - (*rot_matrix)(i, j) = R(i, j); - } - } + rot_matrix->from_euler(roll, pitch, yaw); } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fcb..0c56494c5e 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b66d1dba53..9584924ccc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -174,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9e7d35f680..2e86c72dc1 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -141,7 +141,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5e22007277..255776765b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -158,7 +158,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 11def23710..3b68a0a4e8 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); /* * If the AB vector and the vector from B to airplane point in the same @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } } -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const { /* this is an approximation for small angles, proposed by [2] */ - math::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 7a3c42a925..5c0804a392 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5a56dce65e..0f28bccadd 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -30,7 +30,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -282,7 +282,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -505,7 +505,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4fc009da9e..d1ebacda16 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -348,7 +348,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081b..0000000000 --- a/src/lib/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.cpp - * - * math direction cosine matrix - */ - -#include - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3aa..0000000000 --- a/src/lib/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb0..0000000000 --- a/src/lib/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3d..0000000000 --- a/src/lib/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15eca..ea0cf4ca1e 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +35,401 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix.hpp * - * matrix code + * Matrix class */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +template +class __EXPORT Matrix; + +// MxN matrix with float elements +template +class __EXPORT MatrixBase +{ +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + arm_mat = {M, N, &data[0][0]}; + } + + /** + * copyt ctor + */ + MatrixBase(const MatrixBase &m) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, m.data, sizeof(data)); + } + + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + + MatrixBase(const float d[M][N]) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float *d) { + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[M][N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access by index + */ + float &operator()(const unsigned int row, const unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + float operator()(const unsigned int row, const unsigned int col) const { + return data[row][col]; + } + + /** + * get rows number + */ + unsigned int get_rows() const { + return M; + } + + /** + * get columns number + */ + unsigned int get_cols() const { + return N; + } + + /** + * test for equality + */ + bool operator ==(const Matrix &m) const { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; + + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Matrix &m) const { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; + + return false; + } + + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(data, m.data, sizeof(data)); + return *static_cast*>(this); + } + + /** + * negation + */ + Matrix operator -(void) const { + Matrix res; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; + + return res; + } + + /** + * addition + */ + Matrix operator +(const Matrix &m) const { + Matrix res; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; + + return res; + } + + Matrix &operator +=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + + return *static_cast*>(this); + } + + /** + * subtraction + */ + Matrix operator -(const Matrix &m) const { + Matrix res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; + + return res; + } + + Matrix &operator -=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + + return *static_cast*>(this); + } + + /** + * uniform scaling + */ + Matrix operator *(const float num) const { + Matrix res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; + + return res; + } + + Matrix &operator *=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + + return *static_cast*>(this); + } + + Matrix operator /(const float num) const { + Matrix res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + + return res; + } + + Matrix &operator /=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + + return *static_cast*>(this); + } + + /** + * multiplication by another matrix + */ + template + Matrix operator *(const Matrix &m) const { + Matrix res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * transpose the matrix + */ + Matrix transposed(void) const { + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * invert the matrix + */ + Matrix inversed(void) const { + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * set zero matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * set identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + unsigned int n = (M < N) ? M : N; + + for (unsigned int i = 0; i < n; i++) + data[i][i] = 1; + } + + void print(void) { + for (unsigned int i = 0; i < M; i++) { + printf("[ "); + + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + + printf(" ]\n"); + } + } +}; + +template +class __EXPORT Matrix : public MatrixBase +{ +public: + using MatrixBase::operator *; + + Matrix() : MatrixBase() {} + + Matrix(const Matrix &m) : MatrixBase(m) {} + + Matrix(const float *d) : MatrixBase(d) {} + + Matrix(const float d[M][N]) : MatrixBase(d) {} + + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + Vector operator *(const Vector &v) const { + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; + } +}; + +template <> +class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> +{ +public: + using MatrixBase<3, 3>::operator *; + + Matrix() : MatrixBase<3, 3>() {} + + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} + + Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {} + + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + Vector<3> operator *(const Vector<3> &v) const { + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + /** + * get euler angles from rotation matrix + */ + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } + + return euler; + } +}; + +} + +#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca66..0000000000 --- a/src/lib/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d332..21d05c7ef1 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,82 +37,129 @@ /** * @file Quaternion.hpp * - * math quaternion lib + * Quaternion class */ -#pragma once +#ifndef QUATERNION_HPP +#define QUATERNION_HPP +#include +#include "../CMSIS/Include/arm_math.h" #include "Vector.hpp" #include "Matrix.hpp" namespace math { -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector +class __EXPORT Quaternion : public Vector<4> { public: + /** + * trivial ctor + */ + Quaternion() : Vector<4>() {} /** - * default ctor + * copy ctor */ - Quaternion(); + Quaternion(const Quaternion &q) : Vector<4>(q) {} /** - * ctor from floats + * casting from vector */ - Quaternion(float a, float b, float c, float d); + Quaternion(const Vector<4> &v) : Vector<4>(v) {} /** - * ctor from data + * setting ctor */ - Quaternion(const float *data); + Quaternion(const float d[4]) : Vector<4>(d) {} /** - * ctor from Vector + * setting ctor */ - Quaternion(const Vector &v); + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {} + + using Vector<4>::operator *; /** - * ctor from EulerAngles + * multiplication */ - Quaternion(const EulerAngles &euler); - - /** - * ctor from Dcm - */ - Quaternion(const Dcm &dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); - - /** - * dtor - */ - virtual ~Quaternion(); + const Quaternion operator *(const Quaternion &q) const { + return Quaternion( + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + } /** * derivative */ - Vector derivative(const Vector &w); + const Quaternion derivative(const Vector<3> &w) { + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4, 4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; + } /** - * accessors + * imaginary part of quaternion */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } + Vector<3> imag(void) { + return Vector<3>(&data[1]); + } + + /** + * set quaternion to rotation defined by euler angles + */ + void from_euler(float roll, float pitch, float yaw) { + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; + } + + void from_dcm(const Matrix<3, 3> &m) { + // avoiding singularities by not using division equations + data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]); + data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]); + data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]); + data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]); + } + + /** + * create rotation matrix for the quaternion + */ + Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> R; + float aSq = data[0] * data[0]; + float bSq = data[1] * data[1]; + float cSq = data[2] * data[2]; + float dSq = data[3] * data[3]; + R.data[0][0] = aSq + bSq - cSq - dSq; + R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); + R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); + R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); + R.data[1][1] = aSq - bSq + cSq - dSq; + R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); + R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); + R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); + R.data[2][2] = aSq - bSq - cSq + dSq; + return R; + } }; -int __EXPORT quaternionTest(); -} // math +} +#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396c..0000000000 --- a/src/lib/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d51..c7323c2153 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +35,466 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Vector class */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template +class __EXPORT Vector; + +template +class __EXPORT VectorBase +{ +public: + /** + * vector data + */ + float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + VectorBase() { + arm_col = {N, 1, &data[0]}; + } + + /** + * copy ctor + */ + VectorBase(const VectorBase &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } + + /** + * setting ctor + */ + VectorBase(const float d[N]) { + arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access to elements by index + */ + float &operator()(const unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + float operator()(const unsigned int i) const { + return data[i]; + } + + /** + * get vector size + */ + unsigned int get_size() const { + return N; + } + + /** + * test for equality + */ + bool operator ==(const Vector &v) const { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return false; + + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector &v) const { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return true; + + return false; + } + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(data, v.data, sizeof(data)); + return *static_cast*>(this); + } + + /** + * negation + */ + const Vector operator -(void) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = -data[i]; + + return res; + } + + /** + * addition + */ + const Vector operator +(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] + v.data[i]; + + return res; + } + + /** + * subtraction + */ + const Vector operator -(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] - v.data[i]; + + return res; + } + + /** + * uniform scaling + */ + const Vector operator *(const float num) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + + return res; + } + + /** + * uniform scaling + */ + const Vector operator /(const float num) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + + return res; + } + + /** + * addition + */ + const Vector &operator +=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v.data[i]; + + return *static_cast*>(this); + } + + /** + * subtraction + */ + const Vector &operator -=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v.data[i]; + + return *static_cast*>(this); + } + + /** + * uniform scaling + */ + const Vector &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + + return *static_cast*>(this); + } + + /** + * uniform scaling + */ + const Vector &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + + return *static_cast*>(this); + } + + /** + * dot product + */ + float operator *(const Vector &v) const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * v.data[i]; + + return res; + } + + /** + * element by element multiplication + */ + const Vector emult(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * v.data[i]; + + return res; + } + + /** + * element by element division + */ + const Vector edivide(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / v.data[i]; + + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return res; + } + + /** + * gets the length of this vector + */ + float length() const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return sqrtf(res); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector normalized() const { + return *this / length(); + } + + /** + * set zero vector + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + void print(void) { + printf("[ "); + + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + + printf("]\n"); + } +}; + +template +class __EXPORT Vector : public VectorBase +{ +public: + Vector() : VectorBase() {} + + Vector(const Vector &v) : VectorBase(v) {} + + Vector(const float d[N]) : VectorBase(d) {} + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(this->data, v.data, sizeof(this->data)); + return *this; + } +}; + +template <> +class __EXPORT Vector<2> : public VectorBase<2> +{ +public: + Vector() : VectorBase<2>() {} + + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<2> &v) : VectorBase<2>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const float d[2]) : VectorBase<2>() { + data[0] = d[0]; + data[1] = d[1]; + } + + Vector(const float x, const float y) : VectorBase<2>() { + data[0] = x; + data[1] = y; + } + + /** + * set data + */ + void set(const float d[2]) { + data[0] = d[0]; + data[1] = d[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float operator %(const Vector<2> &v) const { + return data[0] * v.data[1] - data[1] * v.data[0]; + } +}; + +template <> +class __EXPORT Vector<3> : public VectorBase<3> +{ +public: + Vector() : VectorBase<3>() {} + + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<3> &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; + } + + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + Vector(const float x, const float y, const float z) : VectorBase<3>() { + data[0] = x; + data[1] = y; + data[2] = z; + } + + /** + * set data + */ + void set(const float d[3]) { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; + + return *this; + } + + Vector<3> operator %(const Vector<3> &v) const { + return Vector<3>( + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); + } +}; + +template <> +class __EXPORT Vector<4> : public VectorBase<4> +{ +public: + Vector() : VectorBase() {} + + Vector(const Vector<4> &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; + } + + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; + } + + Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { + data[0] = x0; + data[1] = x1; + data[2] = x2; + data[3] = x3; + } + + /** + * set data + */ + void set(const float d[4]) { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; + } + + /** + * set to value + */ + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; + + return *this; + } +}; + +} + +#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817d..0000000000 --- a/src/lib/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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. - * - ****************************************************************************/ - -/** - * @file Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600ef..0000000000 --- a/src/lib/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02db..0000000000 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exponent; - float num = (*this)(i, j); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc152..0000000000 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exponent; - float num = (*this)(i); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447b..0000000000 --- a/src/lib/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d7..0000000000 --- a/src/lib/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc4..9e03855c52 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,13 +41,9 @@ #pragma once -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" +#include "math/Vector.hpp" #include "math/Matrix.hpp" #include "math/Quaternion.hpp" -#include "math/Vector.hpp" -#include "math/Vector3.hpp" -#include "math/Vector2f.hpp" #include "math/Limits.hpp" #endif @@ -56,4 +52,4 @@ #include "CMSIS/Include/arm_math.h" -#endif \ No newline at end of file +#endif diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 72bc7db8a1..191e2da739 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,13 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ math/Limits.cpp # @@ -49,13 +42,3 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/lib/version/version.h similarity index 95% rename from src/modules/sdlog2/sdlog2_version.h rename to src/lib/version/version.h index c6a9ba6387..af733aaf06 100644 --- a/src/modules/sdlog2/sdlog2_version.h +++ b/src/lib/version/version.h @@ -33,15 +33,15 @@ ****************************************************************************/ /** - * @file sdlog2_version.h + * @file version.h * * Tools for system version detection. * * @author Anton Babushkin */ -#ifndef SDLOG2_VERSION_H_ -#define SDLOG2_VERSION_H_ +#ifndef VERSION_H_ +#define VERSION_H_ /* GIT_VERSION is defined at build time via a Makefile call to the @@ -59,4 +59,4 @@ #define HW_ARCH "PX4FMU_V2" #endif -#endif /* SDLOG2_VERSION_H_ */ +#endif /* VERSION_H_ */ diff --git a/src/mainpage.dox b/src/mainpage.dox new file mode 100644 index 0000000000..7ca410341e --- /dev/null +++ b/src/mainpage.dox @@ -0,0 +1,9 @@ +/** +\mainpage PX4 Autopilot Flight Control Stack and Middleware + +This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows. + +http://pixhawk.org + + +*/ \ No newline at end of file diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7f..83145ac72d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), - // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), - // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), - // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), - // attitude representations - C_nb(), - q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz @@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -228,8 +222,8 @@ void KalmanNav::update() if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude\n"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -259,8 +253,8 @@ void KalmanNav::update() // lat/lon and not have init map_projection_init(lat0, lon0); _positionInitialized = true; - warnx("initialized EKF state with GPS\n"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(vN), double(vE), double(vD), lat, lon, double(alt)); } @@ -321,13 +315,12 @@ void KalmanNav::updatePublications() _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; - _pos.lat = getLatDegE7(); - _pos.lon = getLonDegE7(); + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication @@ -404,28 +397,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb = q.to_dcm(); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +542,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +570,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +586,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +605,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,17 +622,17 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { // abort correction and return - warnx("numerical failure in att correction\n"); + warnx("numerical failure in att correction"); // reset P matrix to P0 P = P0; return ret_error; @@ -669,7 +659,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +668,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +678,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,17 +688,17 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { // abort correction and return - warnx("numerical failure in gps correction\n"); + warnx("numerical failure in gps correction"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; @@ -735,7 +725,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a65..46ee4b6c8e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription _sensors; /**< sensors sub. */ diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 372b2d162c..3d20d4d2d6 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 4096, + 8192, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..66ec20b954 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -58,9 +58,13 @@ #include #include #include +#include +#include #include #include +#include + #include #include #include @@ -214,6 +218,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -221,12 +229,32 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + uint64_t last_vel_t = 0; + + /* current velocity */ + math::Vector<3> vel; + vel.zero(); + /* previous velocity */ + math::Vector<3> vel_prev; + vel_prev.zero(); + /* actual acceleration (by GPS velocity) in body frame */ + math::Vector<3> acc; + acc.zero(); + /* rotation matrix */ + math::Matrix<3, 3> R; + R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); + /* subscribe to GPS */ + int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* subscribe to GPS */ + int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -265,6 +293,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* rotation matrix for magnetic declination */ + math::Matrix<3, 3> R_decl; + R_decl.identity(); + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -299,6 +331,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -307,6 +342,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + bool gps_updated; + orb_check(sub_gps, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + } + + bool global_pos_updated; + orb_check(sub_global_pos, &global_pos_updated); + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); + } + if (!initialized) { // XXX disabling init for now initialized = true; @@ -352,9 +399,50 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + hrt_abstime vel_t = 0; + bool vel_valid = false; + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + vel_valid = true; + if (gps_updated) { + vel_t = gps.timestamp_velocity; + vel(0) = gps.vel_n_m_s; + vel(1) = gps.vel_e_m_s; + vel(2) = gps.vel_d_m_s; + } + + } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + vel_valid = true; + if (global_pos_updated) { + vel_t = global_pos.timestamp; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; + } + } + + if (vel_valid) { + /* velocity is valid */ + if (vel_t != 0) { + /* velocity updated */ + if (last_vel_t != 0 && vel_t != last_vel_t) { + float vel_dt = (vel_t - last_vel_t) / 1000000.0f; + /* calculate acceleration in body frame */ + acc = R.transposed() * ((vel - vel_prev) / vel_dt); + } + last_vel_t = vel_t; + vel_prev = vel; + } + + } else { + /* velocity is valid, reset acceleration */ + acc.zero(); + vel_prev.zero(); + last_vel_t = 0; + } + + z_k[3] = raw.accelerometer_m_s2[0] - acc(0); + z_k[4] = raw.accelerometer_m_s2[1] - acc(1); + z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { @@ -425,7 +513,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) + if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; @@ -433,10 +521,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -445,12 +532,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - //att.yawspeed =z_k[2] ; + att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); + att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); + /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + /* magnetic declination */ + + math::Matrix<3, 3> R_body = (&Rot_matrix[0]); + R = R_decl * R_body; + /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3cfddf28ed..44f47b47ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -65,6 +65,11 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); +/* magnetic declination, in degrees */ +PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); + +PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -83,6 +88,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->pitch_off = param_find("ATT_PITCH_OFF3"); h->yaw_off = param_find("ATT_YAW_OFF3"); + h->mag_decl = param_find("ATT_MAG_DECL"); + + h->acc_comp = param_find("ATT_ACC_COMP"); + return OK; } @@ -103,5 +112,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->pitch_off, &(p->pitch_off)); param_get(h->yaw_off, &(p->yaw_off)); + param_get(h->mag_decl, &(p->mag_decl)); + + param_get(h->acc_comp, &(p->acc_comp)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 09817d58e6..74a1416091 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -47,12 +47,16 @@ struct attitude_estimator_ekf_params { float roll_off; float pitch_off; float yaw_off; + float mag_decl; + int acc_comp; }; struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; + param_t mag_decl; + param_t acc_comp; }; /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a3..36b75dd58a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix board_rotation(3, 3); + math::Matrix<3,3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix board_rotation_t = board_rotation.transpose(); - math::Vector3 accel_offs_vec; - accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix accel_T_mat(3, 3); - accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e53181217..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -106,14 +106,9 @@ static const int ERROR = -1; extern struct system_load_s system_load; -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define MAVLINK_OPEN_INTERVAL 50000 @@ -204,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void print_reject_mode(const char *msg); @@ -376,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -410,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -452,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) ret = true; @@ -491,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -513,11 +512,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -566,7 +565,6 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -580,6 +578,27 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -609,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -666,8 +686,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ unsigned counter = 0; - unsigned low_voltage_counter = 0; - unsigned critical_voltage_counter = 0; unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; @@ -745,7 +763,6 @@ int commander_thread_main(int argc, char *argv[]) int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; /* Subscribe to subsystem info topic */ int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); @@ -889,13 +906,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - // warnx("bat v: %2.2f", battery.voltage_v); - - /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { - status.battery_voltage = battery.voltage_v; + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + status.battery_voltage = battery.voltage_filtered_v; + status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); } } @@ -948,46 +964,29 @@ int commander_thread_main(int argc, char *argv[]) //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - // XXX remove later - //warnx("bat remaining: %2.2f", status.battery_remaining); - /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { - //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; - status_changed = true; - battery_tune_played = false; - } - - low_voltage_counter++; + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status_changed = true; + battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + battery_tune_played = false; - if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + if (armed.armed) { + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); - } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } - - status_changed = true; + } else { + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } - critical_voltage_counter++; - - } else { - - low_voltage_counter = 0; - critical_voltage_counter = 0; + status_changed = true; } /* End battery voltage check */ @@ -1029,19 +1028,18 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed && global_position.valid) { + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { @@ -1052,129 +1050,163 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + /* start RC input check */ + if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; + + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } } + } - status.rc_signal_lost = false; + status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t res; // store all transitions results here - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * do it only for rotary wings */ - if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - } else { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; - } - - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } - - stick_on_counter = 0; - - } else { - stick_on_counter++; - } } else { - stick_on_counter = 0; - } - - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } - - } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } - - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); - - /* evaluate the main state machine */ - res = check_main_state_machine(&status); - - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); - - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + stick_off_counter++; } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; + stick_off_counter = 0; + } + + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + } + + stick_on_counter = 0; + + } else { + stick_on_counter++; + } + + } else { + stick_on_counter = 0; + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } + + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + } + + /* fill status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine according to mode switches */ + res = set_main_state_rc(&status); + + if (res == TRANSITION_CHANGED) { + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + } + + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1190,13 +1222,24 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + /* print new state */ + if (arming_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1375,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { + switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state @@ -1449,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status) print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages print_reject_mode("SEATBELT"); // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 565b4b66ab..21a1c4c2c7 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } -float battery_remaining_estimate_voltage(float voltage) +float battery_remaining_estimate_voltage(float voltage, float discharged) { float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; + static param_t bat_v_empty_h; + static param_t bat_v_full_h; + static param_t bat_n_cells_h; + static param_t bat_capacity_h; + static float bat_v_empty = 3.2f; + static float bat_v_full = 4.0f; + static int bat_n_cells = 3; + static float bat_capacity = -1.0f; static bool initialized = false; static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); + bat_v_empty_h = param_find("BAT_V_EMPTY"); + bat_v_full_h = param_find("BAT_V_FULL"); + bat_n_cells_h = param_find("BAT_N_CELLS"); + bat_capacity_h = param_find("BAT_CAPACITY"); initialized = true; } - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); + param_get(bat_v_empty_h, &bat_v_empty); + param_get(bat_v_full_h, &bat_v_full); + param_get(bat_n_cells_h, &bat_n_cells); + param_get(bat_capacity_h, &bat_capacity); } counter++; - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + /* remaining charge estimate based on voltage */ + float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + + if (bat_capacity > 0.0f) { + /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ + ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { + /* else use voltage */ + ret = remaining_voltage; + } /* limit to sane values */ ret = (ret < 0.0f) ? 0.0f : ret; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e9514446c1..d0393f45a9 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode); void rgbled_set_pattern(rgbled_pattern_t *pattern); /** - * Provides a coarse estimate of remaining battery power. + * Estimate remaining battery charge. * - * The estimate is very basic and based on decharging voltage curves. + * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), + * else use simple estimate based on voltage. * * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage); +float battery_remaining_estimate_voltage(float voltage, float discharged); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 691d3efcb0..d3155f7bf5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,6 +48,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ffc..c7256583a3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,11 +63,11 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -219,55 +220,54 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_EASY: - - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_AUTO: - - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; } - if (ret == TRANSITION_CHANGED) { - current_state->main_state = new_main_state; + break; + + case MAIN_STATE_EASY: + + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_AUTO: + + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,63 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } } - return ret; + } else { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ + ret = TRANSITION_CHANGED; + break; + + case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_TERMINATION: + /* always allowed */ + ret = TRANSITION_CHANGED; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } + } + } + + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f37..f04879ff9e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e213ac17fc..e8fecef0d9 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 8cc0d77d47..7c80c4b2ba 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd); + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription _attCmd; UOrbSubscription _ratesCmd; UOrbSubscription _pos; - UOrbSubscription _missionCmd; + UOrbSubscription _missionCmd; UOrbSubscription _manual; UOrbSubscription _status; UOrbSubscription _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d6..f7c0b61488 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index b4dbc36b09..e1c85c2618 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - mission_item_triplet_s _lastMissionCmd; + position_setpoint_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e0..888dd09423 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c94180d681..17b1028f9a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { @@ -637,7 +637,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 04caf0bbc3..a62b53221c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _mission_item_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -145,7 +145,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -175,7 +175,6 @@ private: /* takeoff/launch states */ bool launch_detected; bool usePreTakeoffThrust; - bool launch_detection_message_sent; /* Landingslope object */ Landingslope landingslope; @@ -193,7 +192,7 @@ private: uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid - math::Dcm _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -332,11 +331,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, - const struct mission_item_triplet_s &_mission_item_triplet); + bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -368,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _mission_item_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -397,8 +396,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false), - launch_detection_message_sent(false) + usePreTakeoffThrust(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -408,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - mission_item_triplet_s _mission_item_triplet = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; sensor_combined_s _sensor_combined = {0}; @@ -655,11 +653,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool mission_item_triplet_updated; - orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (mission_item_triplet_updated) { - orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); _setpoint_valid = true; } } @@ -702,25 +700,25 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid) { /* rotate ground speed vector with current attitude */ - math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; - if (mission_item_triplet.previous_valid) { - distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { - distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); @@ -752,12 +750,12 @@ void FixedwingPositionControl::navigation_capabilities_publish() } bool -FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, - const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); + calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -765,11 +763,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); - math::Vector3 accel_earth = _R_nb * accel_body; + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -787,70 +785,68 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ - math::Vector2f prev_wp; + math::Vector<2> prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp.setX(mission_item_triplet.previous.lat); - prev_wp.setY(mission_item_triplet.previous.lon); + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp.setX(mission_item_triplet.current.lat); - prev_wp.setY(mission_item_triplet.current.lon); + prev_wp(0) = pos_sp_triplet.current.lat; + prev_wp(1) = pos_sp_triplet.current.lon; } - if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (mission_item_triplet.previous_valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()); + if (pos_sp_triplet.previous.valid) { + target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; } @@ -881,23 +877,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // /* do not go down too early */ // if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; // } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -916,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = mission_item_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; } @@ -979,16 +975,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { -// warnx("Launch detection enabled"); - if(!launch_detection_message_sent) { + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { +// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - launch_detection_message_sent = true; + last_sent = hrt_absolute_time(); } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { @@ -1012,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -1023,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1037,15 +1034,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset land state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1054,10 +1051,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } /* reset takeoff/launch state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; - launch_detection_message_sent = false; } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1178,7 +1174,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1266,14 +1262,14 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _mission_item_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1287,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9f46b51706..7954d75c20 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 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 @@ -49,21 +48,75 @@ * */ +/** + * L1 period + * + * This is the L1 distance and defines the tracking + * point ahead of the aircraft its following. + * A value of 25 meters works for most aircraft. Shorten + * slowly during tuning until response is sharp without oscillation. + * + * @min 1.0 + * @max 100.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); - +/** + * L1 damping + * + * Damping factor for L1 control. + * + * @min 0.6 + * @max 0.9 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); - +/** + * Default Loiter Radius + * + * This radius is used when no other loiter radius is set. + * + * @min 10.0 + * @max 100.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); - +/** + * Cruise throttle + * + * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); - +/** + * Negative pitch limit + * + * The minimum negative pitch the controller will output. + * + * @unit degrees + * @min -60.0 + * @max 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); - +/** + * Positive pitch limit + * + * The maximum positive pitch the controller will output. + * + * @unit degrees + * @min 0.0 + * @max 60.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index e2675dfa71..44bf77bb0d 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -59,7 +59,6 @@ #include "mavlink_orb_listener.h" #include "mavlink_main.h" -//#include "util.h" void *uorb_receive_thread(void *arg); @@ -200,10 +199,10 @@ MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + 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->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -333,13 +332,13 @@ MavlinkOrbListener::l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.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); } @@ -363,23 +362,18 @@ MavlinkOrbListener::l_local_position(const struct listener *l) void MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { - struct mission_item_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); + struct position_setpoint_triplet_s triplet; + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (!triplet.current_valid) + if (!triplet.current.valid) return; - if (triplet.current.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - if (gcs_link) mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), - coordinate_frame, + MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), + (int32_t)(triplet.current.alt * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } @@ -615,11 +609,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) void MavlinkOrbListener::l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void @@ -757,7 +749,7 @@ MavlinkOrbListener::uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + 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 --- */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ab4074558a..4f763c3c68 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -555,6 +555,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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 @@ -566,6 +567,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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 @@ -613,9 +615,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + 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 { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); @@ -659,8 +661,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -675,9 +677,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + 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; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 556b6f8ad6..ea57714d2d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,9 +53,8 @@ #include #include #include -#include +#include #include -#include #include #include #include diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 86bfa26f25..bbc9f6e66a 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp new file mode 100644 index 0000000000..245ac024b8 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -0,0 +1,779 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * 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 mc_att_control_main.c + * Multicopter attitude controller. + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +#define MIN_TAKEOFF_THROTTLE 0.3f +#define YAW_DEADZONE 0.01f +#define RATES_I_LIMIT 0.5f + +class MulticopterAttitudeControl +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + + struct { + param_t att_p; + param_t yaw_p; + param_t att_rate_p; + param_t att_rate_i; + param_t att_rate_d; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + } _params; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterAttitudeControl *g_control; +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + +/* publications */ + _att_sp_pub(-1), + _rates_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + + _params.p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + + _rates_prev.zero(); + + _params_handles.att_p = param_find("MC_ATT_P"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); + _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + + /* fetch initial parameter values */ + parameters_update(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms 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) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + param_get(_params_handles.att_p, &v); + _params.p(0) = v; + _params.p(1) = v; + param_get(_params_handles.yaw_p, &v); + _params.p(2) = v; + + param_get(_params_handles.att_rate_p, &v); + _params.rate_p(0) = v; + _params.rate_p(1) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + + param_get(_params_handles.att_rate_i, &v); + _params.rate_i(0) = v; + _params.rate_i(1) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + + param_get(_params_handles.att_rate_d, &v); + _params.rate_d(0) = v; + _params.rate_d(1) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + return OK; +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool control_mode_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_control_mode_sub, &control_mode_updated); + + if (control_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +void +MulticopterAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool arming_updated; + orb_check(_arming_sub, &arming_updated); + + if (arming_updated) { + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } +} + +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +MulticopterAttitudeControl::task_main() +{ + /* inform about start */ + warnx("started"); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit attitude updates to 100Hz */ + orb_set_interval(_att_sub, 10); + + parameters_update(); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + vehicle_setpoint_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + arming_status_poll(); + + /* setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + R_sp.identity(); + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.identity(); + + /* current angular rates */ + math::Vector<3> rates; + rates.zero(); + + /* angular rates integral error */ + math::Vector<3> rates_int; + rates_int.zero(); + + /* identity matrix */ + math::Matrix<3, 3> I; + I.identity(); + + math::Quaternion q; + + bool reset_yaw_sp = true; + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* copy the topic to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large dt's */ + if (dt > 0.02f) + dt = 0.02f; + + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + vehicle_setpoint_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } + + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } + + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + _att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; + } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } + + if (_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + } + + if (publish_att_sp) { + /* publish the attitude setpoint */ + _att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + } + + /* rotation matrix for current state */ + R.set(_att.R); + + /* current body angular rates */ + rates(0) = _att.rollspeed; + rates(1) = _att.pitchspeed; + rates(2) = _att.yawspeed; + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + q.from_dcm(R.transposed() * R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* angular rates setpoint*/ + math::Vector<3> rates_sp = _params.p.emult(e_R); + + /* feed forward yaw setpoint rate */ + rates_sp(2) += yaw_sp_move_rate * yaw_w; + + /* reset integral if disarmed */ + // TODO add LANDED flag here + if (!_arming.armed) { + rates_int.zero(); + } + + /* rate controller */ + math::Vector<3> rates_err = rates_sp - rates; + math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; + _rates_prev = rates; + + /* update integral */ + for (int i = 0; i < 3; i++) { + float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i)) { + if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { + rates_int(i) = rate_i; + } + } + } + + /* publish the attitude rates setpoint */ + _rates_sp.roll = rates_sp(0); + _rates_sp.pitch = rates_sp(1); + _rates_sp.yaw = rates_sp(2); + _rates_sp.thrust = _att_sp.thrust; + _rates_sp.timestamp = hrt_absolute_time(); + + if (_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); + + } else { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } + + /* publish the attitude controls */ + if (_control_mode.flag_control_rates_enabled) { + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + } else { + /* controller disabled, publish zero attitude controls */ + _actuators.control[0] = 0.0f; + _actuators.control[1] = 0.0f; + _actuators.control[2] = 0.0f; + _actuators.control[3] = 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + + perf_end(_loop_perf); + } + + warnx("exit"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new MulticopterAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/modules/mc_att_control/mc_att_control_params.c similarity index 68% rename from src/lib/mathlib/math/arm/Vector.cpp rename to src/modules/mc_att_control/mc_att_control_params.c index 7ea6496bb1..a170365eec 100644 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +35,19 @@ ****************************************************************************/ /** - * @file Vector.cpp - * - * math vector + * @file mc_att_control_params.c + * Parameters for multicopter attitude controller. */ -#include "Vector.hpp" +#include + +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_att_control/module.mk similarity index 87% rename from src/modules/multirotor_pos_control/module.mk rename to src/modules/mc_att_control/module.mk index bc4b48fb40..64b876f69b 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013, 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 @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor position control +# Multirotor attitude controller (vector based, no Euler singularities) # -MODULE_COMMAND = multirotor_pos_control +MODULE_COMMAND = mc_att_control -SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c \ - thrust_pid.c +SRCS = mc_att_control_main.cpp \ + mc_att_control_params.c diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp new file mode 100644 index 0000000000..d3e39e3a02 --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -0,0 +1,1031 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 mc_pos_control_main.cpp + * Multicopter position controller. + * + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f + +/** + * Multicopter position control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + int _local_pos_sub; /**< vehicle local position */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ + + orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + + struct { + param_t thr_min; + param_t thr_max; + param_t z_p; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t z_ff; + param_t xy_p; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; + param_t xy_ff; + param_t tilt_max; + param_t land_speed; + param_t land_tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max; + float land_speed; + float land_tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + math::Vector<3> _pos; + math::Vector<3> _vel; + math::Vector<3> _pos_sp; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + + /** + * Update our local parameter cache. + */ + int parameters_update(bool force); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in subscribed topics. + */ + void poll_subscriptions(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace pos_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterPositionControl *g_control; +} + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + _local_pos_sub(-1), + _pos_sp_triplet_sub(-1), + +/* publications */ + _local_pos_sp_pub(-1), + _att_sp_pub(-1), + _global_vel_sp_pub(-1) +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); + memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _vel.zero(); + _pos_sp.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + + _params_handles.thr_min = param_find("MPC_THR_MIN"); + _params_handles.thr_max = param_find("MPC_THR_MAX"); + _params_handles.z_p = param_find("MPC_Z_P"); + _params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); + _params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); + _params_handles.z_vel_d = param_find("MPC_Z_VEL_D"); + _params_handles.z_vel_max = param_find("MPC_Z_VEL_MAX"); + _params_handles.z_ff = param_find("MPC_Z_FF"); + _params_handles.xy_p = param_find("MPC_XY_P"); + _params_handles.xy_vel_p = param_find("MPC_XY_VEL_P"); + _params_handles.xy_vel_i = param_find("MPC_XY_VEL_I"); + _params_handles.xy_vel_d = param_find("MPC_XY_VEL_D"); + _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); + _params_handles.xy_ff = param_find("MPC_XY_FF"); + _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + + /* fetch initial parameter values */ + parameters_update(true); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms 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) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + pos_control::g_control = nullptr; +} + +int +MulticopterPositionControl::parameters_update(bool force) +{ + bool updated; + struct parameter_update_s param_upd; + + orb_check(_params_sub, &updated); + + if (updated) + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + + if (updated || force) { + param_get(_params_handles.thr_min, &_params.thr_min); + param_get(_params_handles.thr_max, &_params.thr_max); + param_get(_params_handles.tilt_max, &_params.tilt_max); + param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); + param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); + param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + + float v; + param_get(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + param_get(_params_handles.z_p, &v); + _params.pos_p(2) = v; + param_get(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + param_get(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + param_get(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + param_get(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + param_get(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + param_get(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + param_get(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + param_get(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + param_get(_params_handles.xy_ff, &v); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + param_get(_params_handles.z_ff, &v); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + } + + return OK; +} + +void +MulticopterPositionControl::poll_subscriptions() +{ + bool updated; + + orb_check(_att_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + orb_check(_att_sp_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + + orb_check(_control_mode_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + orb_check(_manual_sub, &updated); + + if (updated) + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + + orb_check(_arming_sub, &updated); + + if (updated) + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); +} + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + pos_control::g_control->task_main(); +} + +void +MulticopterPositionControl::task_main() +{ + warnx("started"); + + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); + + /* + * do subscriptions + */ + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + parameters_update(true); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + poll_subscriptions(); + + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + + hrt_abstime t_prev = 0; + + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + + hrt_abstime ref_timestamp = 0; + int32_t ref_lat = 0.0f; + int32_t ref_lon = 0.0f; + float ref_alt = 0.0f; + + math::Vector<3> sp_move_rate; + sp_move_rate.zero(); + math::Vector<3> thrust_int; + thrust_int.zero(); + math::Matrix<3, 3> R; + R.identity(); + + /* wakeup source */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _local_pos_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + + poll_subscriptions(); + parameters_update(false); + + hrt_abstime t = hrt_absolute_time(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; + + if (_control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode.flag_armed; + + if (_control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_climb_rate_enabled || + _control_mode.flag_control_velocity_enabled) { + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; + + sp_move_rate.zero(); + + if (_local_pos.ref_timestamp != ref_timestamp) { + /* initialize local projection with new reference */ + double lat_home = _local_pos.ref_lat * 1e-7; + double lon_home = _local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + + if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { + /* correct setpoint in manual mode to stay in the same point */ + float ref_change_x = 0.0f; + float ref_change_y = 0.0f; + map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); + _pos_sp(0) += ref_change_x; + _pos_sp(1) += ref_change_y; + _pos_sp(2) += _local_pos.ref_alt - ref_alt; + } + + ref_timestamp = _local_pos.ref_timestamp; + ref_lat = _local_pos.ref_lat; + ref_lon = _local_pos.ref_lon; + ref_alt = _local_pos.ref_alt; + } + + if (_control_mode.flag_control_manual_enabled) { + /* manual control */ + if (_control_mode.flag_control_altitude_enabled) { + /* reset setpoint Z to current altitude if needed */ + if (reset_sp_z) { + reset_sp_z = false; + _pos_sp(2) = _pos(2); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); + } + + /* move altitude setpoint with throttle stick */ + sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset setpoint XY to current position if needed */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + } + + /* move position setpoint with roll/pitch stick */ + sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); + sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + } + + /* limit setpoint move rate */ + float sp_move_norm = sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + sp_move_rate /= sp_move_norm; + } + + /* scale to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); + + /* move position setpoint */ + _pos_sp += sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } + + } else { + /* AUTO */ + if (_pos_sp_triplet.current.valid) { + struct position_setpoint_s current_sp = _pos_sp_triplet.current; + + _pos_sp(2) = -(current_sp.alt - ref_alt); + + map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); + + if (isfinite(current_sp.yaw)) { + _att_sp.yaw_body = current_sp.yaw; + } + + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + + if (reset_sp_z) { + reset_sp_z = false; + _pos_sp(2) = _pos(2); + } + } + } + + /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ + _local_pos_sp.yaw = _att_sp.yaw_body; + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + + if (!_control_mode.flag_control_altitude_enabled) { + reset_sp_z = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode.flag_control_position_enabled) { + reset_sp_xy = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_manual_enabled) { + /* use constant descend rate when landing, ignore altitude setpoint */ + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + /* limit 3D speed only in AUTO mode */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } + } + + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } + + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual.throttle; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + thrust_int(2) = -i; + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + } else { + reset_int_z = true; + } + + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } + + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + } + + _att_sp.thrust = thrust_abs; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } else { + reset_int_z = true; + } + + } else { + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + } + + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_pos_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (pos_control::g_control != nullptr) + errx(1, "already running"); + + pos_control::g_control = new MulticopterPositionControl; + + if (pos_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != pos_control::g_control->start()) { + delete pos_control::g_control; + pos_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (pos_control::g_control == nullptr) + errx(1, "not running"); + + delete pos_control::g_control; + pos_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (pos_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/modules/mc_pos_control/mc_pos_control_params.c similarity index 67% rename from src/lib/mathlib/math/Vector2f.hpp rename to src/modules/mc_pos_control/mc_pos_control_params.c index ecd62e81cb..9eb56545d7 100644 --- a/src/lib/mathlib/math/Vector2f.hpp +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,48 +33,26 @@ ****************************************************************************/ /** - * @file Vector2f.hpp - * - * math 3 vector + * @file mc_pos_control_params.c + * Multicopter position controller parameters. */ -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math +#include +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/mc_pos_control/module.mk old mode 100755 new mode 100644 similarity index 86% rename from src/modules/multirotor_att_control/module.mk rename to src/modules/mc_pos_control/module.mk index 7569e1c7ee..0b566d7bd0 --- a/src/modules/multirotor_att_control/module.mk +++ b/src/modules/mc_pos_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 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 @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor attitude controller +# Build multicopter position controller # -MODULE_COMMAND = multirotor_att_control +MODULE_COMMAND = mc_pos_control -SRCS = multirotor_att_control_main.c \ - multirotor_attitude_control.c \ - multirotor_rate_control.c +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_params.c diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c deleted file mode 100644 index 111e9197f6..0000000000 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Anton Babushkin - * - * 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 multirotor_att_control_main.c - * - * Implementation of multirotor attitude control main loop. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "multirotor_attitude_control.h" -#include "multirotor_rate_control.h" - -__EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static bool thread_should_exit; -static int mc_task; -static bool motor_test_mode = false; -static const float min_takeoff_throttle = 0.3f; -static const float yaw_deadzone = 0.01f; - -static int -mc_thread_main(int argc, char *argv[]) -{ - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_status_s status; - memset(&status, 0, sizeof(status)); - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - /* subscribe */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - - warnx("starting"); - - /* store last control mode to detect mode switches */ - bool control_yaw_position = true; - bool reset_yaw_sp = true; - - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - - } else if (ret > 0) { - /* only run controller if attitude changed */ - perf_begin(mc_loop_perf); - - /* attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - - bool updated; - - /* parameters */ - orb_check(parameter_update_sub, &updated); - - if (updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - /* update parameters */ - } - - /* control mode */ - orb_check(vehicle_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); - } - - /* manual control setpoint */ - orb_check(manual_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); - } - - /* attitude setpoint */ - orb_check(vehicle_attitude_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - } - - /* offboard control setpoint */ - orb_check(offboard_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); - } - - /* vehicle status */ - orb_check(vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); - } - - /* sensors */ - orb_check(sensor_combined_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - } - - /* set flag to safe value */ - control_yaw_position = true; - - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* define which input is the dominating control input */ - if (control_mode.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = true; - - } else if (control_mode.flag_control_manual_enabled) { - /* manual input */ - if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - if (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; - - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } - - } else { - /* only move yaw setpoint if manual input is != 0 */ - if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { - /* control yaw rate */ - control_yaw_position = false; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } - } - - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } - - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; - } - - } else { - if (!control_mode.flag_control_attitude_enabled) { - /* no control, try to stay on place */ - if (!control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } - - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; - } - - /* check if we should we reset integrals */ - bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - - /* run attitude controller if needed */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - - /* run rates controller if needed */ - if (control_mode.flag_control_rates_enabled) { - /* get current rate setpoint */ - bool rates_sp_updated = false; - orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); - - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); - } - - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - rates[1] = att.pitchspeed; - rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); - - } else { - /* rates controller disabled, set actuators to zero for safety */ - actuators.control[0] = 0.0f; - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - actuators.control[3] = 0.0f; - } - - /* fill in manual control values */ - actuators.control[4] = manual.flaps; - actuators.control[5] = manual.aux1; - actuators.control[6] = manual.aux2; - actuators.control[7] = manual.aux3; - - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - perf_end(mc_loop_perf); - } - } - - warnx("stopping, disarming motors"); - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(vehicle_attitude_sub); - close(vehicle_control_mode_sub); - close(manual_control_setpoint_sub); - close(actuator_pub); - close(att_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - exit(0); -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_att_control [-m ] [-t] {start|status|stop}\n"); - fprintf(stderr, " is 'rates' or 'attitude'\n"); - fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); - exit(1); -} - -int multirotor_att_control_main(int argc, char *argv[]) -{ - int ch; - unsigned int optioncount = 0; - - while ((ch = getopt(argc, argv, "tm:")) != EOF) { - switch (ch) { - case 't': - motor_test_mode = true; - optioncount += 1; - break; - - case ':': - usage("missing parameter"); - break; - - default: - fprintf(stderr, "option: -%c\n", ch); - usage("unrecognized option"); - break; - } - } - - argc -= optioncount; - //argv += optioncount; - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1 + optioncount], "start")) { - - thread_should_exit = false; - mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); - exit(0); - } - - if (!strcmp(argv[1 + optioncount], "stop")) { - thread_should_exit = true; - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c deleted file mode 100644 index 8245aa5609..0000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ /dev/null @@ -1,254 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * 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 multirotor_attitude_control.c - * - * Implementation of attitude controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#include "multirotor_attitude_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); - -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); - -struct mc_att_control_params { - float yaw_p; - float yaw_i; - float yaw_d; - //float yaw_awu; - //float yaw_lim; - - float att_p; - float att_i; - float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; -}; - -struct mc_att_control_param_handles { - param_t yaw_p; - param_t yaw_i; - param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; - - param_t att_p; - param_t att_i; - param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); - - -static int parameters_init(struct mc_att_control_param_handles *h) -{ - /* PID parameters */ - h->yaw_p = param_find("MC_YAWPOS_P"); - h->yaw_i = param_find("MC_YAWPOS_I"); - h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); - - h->att_p = param_find("MC_ATT_P"); - h->att_i = param_find("MC_ATT_I"); - h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); - - return OK; -} - -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) -{ - param_get(h->yaw_p, &(p->yaw_p)); - param_get(h->yaw_i, &(p->yaw_i)); - param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); - - param_get(h->att_p, &(p->att_p)); - param_get(h->att_i, &(p->att_i)); - param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); - - return OK; -} - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) -{ - static uint64_t last_run = 0; - static uint64_t last_input = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (last_input != att_sp->timestamp) { - last_input = att_sp->timestamp; - } - - static int motor_skip_counter = 0; - - static PID_t pitch_controller; - static PID_t roll_controller; - - static struct mc_att_control_params p; - static struct mc_att_control_param_handles h; - - static bool initialized = false; - - static float yaw_error; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - - initialized = true; - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - - /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_controller); - pid_reset_integral(&roll_controller); - //TODO pid_reset_integral(&yaw_controller); - } - - /* calculate current control outputs */ - - /* control pitch (forward) output */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); - - /* control roll (left/right) output */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); - - if (control_yaw_position) { - /* control yaw rate */ - // TODO use pid lib - - /* positive error: rotate to right, negative error, rotate to left (NED frame) */ - // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); - - yaw_error = att_sp->yaw_body - att->yaw; - - if (yaw_error > M_PI_F) { - yaw_error -= M_TWOPI_F; - - } else if (yaw_error < -M_PI_F) { - yaw_error += M_TWOPI_F; - } - - rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); - } - - rates_sp->thrust = att_sp->thrust; - //need to update the timestamp now that we've touched rates_sp - rates_sp->timestamp = hrt_absolute_time(); - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h deleted file mode 100644 index 431a435f7d..0000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * 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 multirotor_attitude_control.h - * - * Definition of attitude controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ -#define MULTIROTOR_ATTITUDE_CONTROL_H_ - -#include -#include -#include -#include -#include - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); - -#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c deleted file mode 100644 index 86ac0e4ff0..0000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * Anton Babushkin - * Julian Oes - * - * 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 multirotor_rate_control.c - * - * Implementation of rate controller for multirotors. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Julian Oes - */ - -#include "multirotor_rate_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); - -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); - -struct mc_rate_control_params { - - float yawrate_p; - float yawrate_d; - float yawrate_i; - - float attrate_p; - float attrate_d; - float attrate_i; - - float rate_lim; -}; - -struct mc_rate_control_param_handles { - - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_d; - - param_t attrate_p; - param_t attrate_i; - param_t attrate_d; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_rate_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p); - - -static int parameters_init(struct mc_rate_control_param_handles *h) -{ - /* PID parameters */ - h->yawrate_p = param_find("MC_YAWRATE_P"); - h->yawrate_i = param_find("MC_YAWRATE_I"); - h->yawrate_d = param_find("MC_YAWRATE_D"); - - h->attrate_p = param_find("MC_ATTRATE_P"); - h->attrate_i = param_find("MC_ATTRATE_I"); - h->attrate_d = param_find("MC_ATTRATE_D"); - - return OK; -} - -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p) -{ - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_d, &(p->yawrate_d)); - - param_get(h->attrate_p, &(p->attrate_p)); - param_get(h->attrate_i, &(p->attrate_i)); - param_get(h->attrate_d, &(p->attrate_d)); - - return OK; -} - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral) -{ - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - static uint64_t last_input = 0; - - if (last_input != rate_sp->timestamp) { - last_input = rate_sp->timestamp; - } - - last_run = hrt_absolute_time(); - - static int motor_skip_counter = 0; - - static PID_t pitch_rate_controller; - static PID_t roll_rate_controller; - static PID_t yaw_rate_controller; - - static struct mc_rate_control_params p; - static struct mc_rate_control_param_handles h; - - static bool initialized = false; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - initialized = true; - - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 2500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_rate_controller); - pid_reset_integral(&roll_rate_controller); - pid_reset_integral(&yaw_rate_controller); - } - - /* run pitch, roll and yaw controllers */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_control; - actuators->control[3] = rate_sp->thrust; - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h deleted file mode 100644 index ca7794c59b..0000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * 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 multirotor_attitude_control.h - * - * Definition of rate controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#ifndef MULTIROTOR_RATE_CONTROL_H_ -#define MULTIROTOR_RATE_CONTROL_H_ - -#include -#include -#include -#include - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral); - -#endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c deleted file mode 100644 index 2ca6504208..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ /dev/null @@ -1,553 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control.c - * - * Multirotor position controller - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "multirotor_pos_control_params.h" -#include "thrust_pid.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int multirotor_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static float scale_control(float ctl, float end, float dz); - -static float norm(float x, float y); - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn(). - */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - warnx("start"); - thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("app is running"); - - } else { - warnx("app not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static float scale_control(float ctl, float end, float dz) -{ - if (ctl > dz) { - return (ctl - dz) / (end - dz); - - } else if (ctl < -dz) { - return (ctl + dz) / (end - dz); - - } else { - return 0.0f; - } -} - -static float norm(float x, float y) -{ - return sqrtf(x * x + y * y); -} - -static int multirotor_pos_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); - - /* structures */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct mission_item_triplet_s triplet; - memset(&triplet, 0, sizeof(triplet)); - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(global_vel_sp)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); - - /* publish setpoint */ - orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; - bool reset_int_z = true; - bool reset_int_z_manual = false; - bool reset_int_xy = true; - bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; - bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; - - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - uint64_t local_ref_timestamp = 0; - - PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; - PID_t z_pos_pid; - thrust_pid_t z_vel_pid; - - thread_running = true; - - struct multirotor_position_control_params params; - struct multirotor_position_control_param_handles params_h; - parameters_init(¶ms_h); - parameters_update(¶ms_h, ¶ms); - - - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - } - - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - - while (!thread_should_exit) { - - bool param_updated; - orb_check(param_sub, ¶m_updated); - - if (param_updated) { - /* clear updated flag */ - struct parameter_update_s ps; - orb_copy(ORB_ID(parameter_update), param_sub, &ps); - /* update params */ - parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); - } - - bool updated; - - orb_check(control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } - - orb_check(mission_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); - global_pos_sp_valid = triplet.current_valid; - reset_mission_sp = true; - } - - hrt_abstime t = hrt_absolute_time(); - float dt; - - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - - } else { - dt = 0.0f; - } - - if (control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; - reset_takeoff_sp = true; - reset_int_z = true; - reset_int_xy = true; - } - - was_armed = control_mode.flag_armed; - - t_prev = t; - - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; - float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ - if (control_mode.flag_control_altitude_enabled) { - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); - } - - /* move altitude setpoint with throttle stick */ - float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - - if (z_sp_ctl != 0.0f) { - sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; - local_pos_sp.z += sp_move_rate[2] * dt; - - if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { - local_pos_sp.z = local_pos.z + z_sp_offs_max; - - } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { - local_pos_sp.z = local_pos.z - z_sp_offs_max; - } - } - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - /* move position setpoint with roll/pitch stick */ - float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - - if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { - /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; - sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - local_pos_sp.x += sp_move_rate[0] * dt; - local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction - * fail safe, should not happen in normal operation */ - float pos_vec_x = local_pos_sp.x - local_pos.x; - float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; - - if (pos_vec_norm > 1.0f) { - local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; - local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; - } - } - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !control_mode.flag_control_position_enabled; - reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; - reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; - } - /* AUTO not implemented */ - - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - - /* run position & altitude controllers, calculate velocity setpoint */ - if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; - - } else { - reset_man_sp_z = true; - global_vel_sp.vz = 0.0f; - } - - if (control_mode.flag_control_position_enabled) { - /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; - - /* limit horizontal speed */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - - } else { - reset_man_sp_xy = true; - global_vel_sp.vx = 0.0f; - global_vel_sp.vy = 0.0f; - } - - /* publish new velocity setpoint */ - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subscribe to velocity setpoint if altitude/position control disabled - - if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ - float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - - if (reset_int_z_manual) { - i = manual.throttle; - - if (i < params.thr_min) { - i = params.thr_min; - - } else if (i > params.thr_max) { - i = params.thr_max; - } - } - - thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); - att_sp.thrust = -thrust_sp[2]; - - } else { - /* reset thrust integral when altitude control enabled */ - reset_int_z = true; - } - - if (control_mode.flag_control_velocity_enabled) { - /* calculate thrust set point in NED frame */ - if (reset_int_xy) { - reset_int_xy = false; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); - } - - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); - - /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ - /* limit horizontal part of thrust */ - float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - /* assuming that vertical component of thrust is g, - * horizontal component = g * tan(alpha) */ - float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - - if (tilt > params.tilt_max) { - tilt = params.tilt_max; - } - - /* convert direction to body frame */ - thrust_xy_dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * tilt; - att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); - - } else { - reset_int_xy = true; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - } else { - /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_int_z = true; - reset_int_xy = true; - reset_mission_sp = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - - /* run at approximately 50 Hz */ - usleep(20000); - } - - warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); - - thread_running = false; - - fflush(stdout); - return 0; -} - diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c deleted file mode 100644 index b7041e4d57..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control_params.c - * - * Parameters for multirotor_pos_control - */ - -#include "multirotor_pos_control_params.h" - -/* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); -PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); -PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); - -int parameters_init(struct multirotor_position_control_param_handles *h) -{ - h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); - h->thr_min = param_find("MPC_THR_MIN"); - h->thr_max = param_find("MPC_THR_MAX"); - h->z_p = param_find("MPC_Z_P"); - h->z_d = param_find("MPC_Z_D"); - h->z_vel_p = param_find("MPC_Z_VEL_P"); - h->z_vel_i = param_find("MPC_Z_VEL_I"); - h->z_vel_d = param_find("MPC_Z_VEL_D"); - h->z_vel_max = param_find("MPC_Z_VEL_MAX"); - h->xy_p = param_find("MPC_XY_P"); - h->xy_d = param_find("MPC_XY_D"); - h->xy_vel_p = param_find("MPC_XY_VEL_P"); - h->xy_vel_i = param_find("MPC_XY_VEL_I"); - h->xy_vel_d = param_find("MPC_XY_VEL_D"); - h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->tilt_max = param_find("MPC_TILT_MAX"); - - h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); - h->rc_scale_roll = param_find("RC_SCALE_ROLL"); - h->rc_scale_yaw = param_find("RC_SCALE_YAW"); - - return OK; -} - -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) -{ - param_get(h->takeoff_alt, &(p->takeoff_alt)); - param_get(h->takeoff_gap, &(p->takeoff_gap)); - param_get(h->thr_min, &(p->thr_min)); - param_get(h->thr_max, &(p->thr_max)); - param_get(h->z_p, &(p->z_p)); - param_get(h->z_d, &(p->z_d)); - param_get(h->z_vel_p, &(p->z_vel_p)); - param_get(h->z_vel_i, &(p->z_vel_i)); - param_get(h->z_vel_d, &(p->z_vel_d)); - param_get(h->z_vel_max, &(p->z_vel_max)); - param_get(h->xy_p, &(p->xy_p)); - param_get(h->xy_d, &(p->xy_d)); - param_get(h->xy_vel_p, &(p->xy_vel_p)); - param_get(h->xy_vel_i, &(p->xy_vel_i)); - param_get(h->xy_vel_d, &(p->xy_vel_d)); - param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->tilt_max, &(p->tilt_max)); - - param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); - param_get(h->rc_scale_roll, &(p->rc_scale_roll)); - param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); - - return OK; -} diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h deleted file mode 100644 index fc658dadb8..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control_params.h - * - * Parameters for multirotor_pos_control - */ - -#include - -struct multirotor_position_control_params { - float takeoff_alt; - float takeoff_gap; - float thr_min; - float thr_max; - float z_p; - float z_d; - float z_vel_p; - float z_vel_i; - float z_vel_d; - float z_vel_max; - float xy_p; - float xy_d; - float xy_vel_p; - float xy_vel_i; - float xy_vel_d; - float xy_vel_max; - float tilt_max; - - float rc_scale_pitch; - float rc_scale_roll; - float rc_scale_yaw; -}; - -struct multirotor_position_control_param_handles { - param_t takeoff_alt; - param_t takeoff_gap; - param_t thr_min; - param_t thr_max; - param_t z_p; - param_t z_d; - param_t z_vel_p; - param_t z_vel_i; - param_t z_vel_d; - param_t z_vel_max; - param_t xy_p; - param_t xy_d; - param_t xy_vel_p; - param_t xy_vel_i; - param_t xy_vel_d; - param_t xy_vel_max; - param_t tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; - param_t rc_scale_yaw; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct multirotor_position_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c deleted file mode 100644 index b985630aec..0000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ /dev/null @@ -1,189 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 thrust_pid.c - * - * Implementation of thrust control PID. - * - * @author Anton Babushkin - */ - -#include "thrust_pid.h" -#include - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) -{ - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->limit_min = limit_min; - pid->limit_max = limit_max; - pid->mode = mode; - pid->dt_min = dt_min; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; - pid->integral = 0.0f; -} - -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) -{ - int ret = 0; - - if (isfinite(kp)) { - pid->kp = kp; - - } else { - ret = 1; - } - - if (isfinite(ki)) { - pid->ki = ki; - - } else { - ret = 1; - } - - if (isfinite(kd)) { - pid->kd = kd; - - } else { - ret = 1; - } - - if (isfinite(limit_min)) { - pid->limit_min = limit_min; - - } else { - ret = 1; - } - - if (isfinite(limit_max)) { - pid->limit_max = limit_max; - - } else { - ret = 1; - } - - return ret; -} - -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) -{ - /* Alternative integral component calculation - * - * start: - * error = setpoint - current_value - * integral = integral + (Ki * error * dt) - * derivative = (error - previous_error) / dt - * previous_error = error - * output = (Kp * error) + integral + (Kd * derivative) - * wait(dt) - * goto start - */ - - if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { - return pid->last_output; - } - - float i, d; - pid->sp = sp; - - // Calculated current error value - float error = pid->sp - val; - - // Calculate or measured current error derivative - if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; - - } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else { - d = 0.0f; - } - - if (!isfinite(d)) { - d = 0.0f; - } - - /* calculate the error integral */ - i = pid->integral + (pid->ki * error * dt); - - /* attitude-thrust compensation - * r22 is (2, 2) componet of rotation matrix for current attitude */ - float att_comp; - - if (r22 > 0.8f) - att_comp = 1.0f / r22; - else if (r22 > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; - else - att_comp = 1.0f; - - /* calculate output */ - float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; - - /* check for saturation */ - if (output < pid->limit_min || output > pid->limit_max) { - /* saturated, recalculate output with old integral */ - output = (error * pid->kp) + pid->integral + (d * pid->kd); - - } else { - if (isfinite(i)) { - pid->integral = i; - } - } - - if (isfinite(output)) { - if (output > pid->limit_max) { - output = pid->limit_max; - - } else if (output < pid->limit_min) { - output = pid->limit_min; - } - - pid->last_output = output; - } - - return pid->last_output; -} - -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) -{ - pid->integral = i; -} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h deleted file mode 100644 index 5e169c1ba5..0000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 thrust_pid.h - * - * Definition of thrust control PID interface. - * - * @author Anton Babushkin - */ - -#ifndef THRUST_PID_H_ -#define THRUST_PID_H_ - -#include - -__BEGIN_DECLS - -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ -#define THRUST_PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ -#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 - -typedef struct { - float kp; - float ki; - float kd; - float sp; - float integral; - float error_previous; - float last_output; - float limit_min; - float limit_max; - float dt_min; - uint8_t mode; -} thrust_pid_t; - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); - -__END_DECLS - -#endif /* THRUST_PID_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 63952be6ee..9c5e62be7a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -62,7 +62,7 @@ #include #include #include -#include +#include #include #include #include @@ -150,7 +150,7 @@ private: int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -158,16 +158,18 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct mission_item_s _mission_item; /**< current mission item */ + bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -184,6 +186,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + char *nav_states_str[NAV_STATE_MAX]; + struct { float min_altitude; float acceptance_radius; @@ -192,6 +196,7 @@ private: float takeoff_alt; float land_alt; float rtl_alt; + float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { @@ -202,6 +207,7 @@ private: param_t takeoff_alt; param_t land_alt; param_t rtl_alt; + param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -210,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -286,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -311,7 +318,7 @@ private: /** * Move to next waypoint */ - void advance_mission(); + void set_mission_item(); /** * Switch to next RTL state @@ -319,35 +326,24 @@ private: void set_rtl_item(); /** - * Helper function to get a loiter item + * Set position setpoint for mission item */ - void get_loiter_item(mission_item_s *item); + void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); /** * Helper function to get a takeoff item */ - void get_takeoff_item(mission_item_s *item); + void get_takeoff_setpoint(position_setpoint_s *pos_sp); /** * Publish a new mission item triplet for position controller */ - void publish_mission_item_triplet(); + void publish_position_setpoint_triplet(); /** * Publish vehicle_control_mode topic for controllers */ void publish_control_mode(); - - - /** - * Compare two mission items if they are equivalent - * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. - * - * @return true if equivalent, false otherwise - */ - bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); - - void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -362,7 +358,7 @@ namespace navigator Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -381,7 +377,7 @@ Navigator::Navigator() : _capabilities_sub(-1), /* publications */ - _triplet_pub(-1), + _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -398,6 +394,7 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), + _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), _geofence_violation_warning_sent(false) @@ -409,15 +406,19 @@ Navigator::Navigator() : _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); + _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); - + 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)); + nav_states_str[0] = "NONE"; + nav_states_str[1] = "READY"; + nav_states_str[2] = "LOITER"; + nav_states_str[3] = "MISSION"; + nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -463,6 +464,7 @@ Navigator::parameters_update() param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); + param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); @@ -472,7 +474,6 @@ Navigator::parameters_update() void Navigator::global_position_update() { - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } @@ -493,16 +494,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -519,6 +524,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -561,11 +567,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -581,7 +589,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -647,87 +655,122 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - stick_mode = true; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + stick_mode = true; + } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + stick_mode = true; + } + + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } } - stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); } else { - dispatch(EVENT_LOITER_REQUESTED); - } - break; + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; } } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + + dispatch(EVENT_RTL_REQUESTED); + } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -746,7 +789,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -767,6 +810,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -775,15 +819,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -792,7 +836,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } @@ -833,128 +877,153 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* NAV_STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + { + /* NAV_STATE_READY */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* NAV_STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* NAV_STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* NAV_STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + }, + { + /* NAV_STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, }, }; void Navigator::start_none() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_ready() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; - // TODO check if (_rtl_state != RTL_STATE_LAND) { + /* allow RTL if landed not at home */ _rtl_state = RTL_STATE_NONE; } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -963,33 +1032,41 @@ Navigator::start_loiter() _do_takeoff = false; /* set loiter position if needed */ - if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _mission_item_triplet.current.altitude_is_relative = false; - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { - _mission_item_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } + + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + + if (_rtl_state == RTL_STATE_LAND) { + /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ + _rtl_state = RTL_STATE_DESCEND; + } } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; - get_loiter_item(&_mission_item_triplet.current); - - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -997,16 +1074,14 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); - advance_mission(); + set_mission_item(); } void -Navigator::advance_mission() +Navigator::set_mission_item() { /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _reset_loiter_pos = true; _do_takeoff = false; @@ -1015,36 +1090,34 @@ Navigator::advance_mission() bool onboard; unsigned index; - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - takeoff_alt_amsl += _home_pos.altitude; + float takeoff_alt_amsl = _pos_sp_triplet.current.alt; if (_vstatus.condition_landed) { /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ @@ -1052,210 +1125,305 @@ Navigator::advance_mission() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + /* move current position setpoint to next */ + memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - /* set current item to TAKEOFF */ - get_takeoff_item(&_mission_item_triplet.current); + /* set current setpoint to takeoff */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = takeoff_alt_amsl; - _mission_item_triplet.current.altitude_is_relative = false; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = takeoff_alt_amsl; + _pos_sp_triplet.current.yaw = NAN; + _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_triplet.current_valid = false; + _mission_item_valid = false; + _pos_sp_triplet.current.valid = false; warnx("ERROR: current WP can't be set"); } if (!_do_takeoff) { - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + mission_item_s item_next; + ret = _mission.get_next_mission_item(&item_next); if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + } else { /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.next.valid = false; } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_rtl() { - _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state == RTL_STATE_NONE) - _rtl_state = RTL_STATE_CLIMB; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); + if (_rtl_state == RTL_STATE_NONE) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + + if (_reset_loiter_pos) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + } + } + + _reset_loiter_pos = true; set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; +} + void Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + float climb_alt = _home_pos.alt + _parameters.rtl_alt; - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + if (_vstatus.condition_landed) { + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); - break; - } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); +} + +void +Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +{ + sp->valid = true; + + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* set home position for RTL item */ + sp->lat = _home_pos.lat; + sp->lon = _home_pos.lon; + sp->alt = _home_pos.alt + _parameters.rtl_alt; + + } else { + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; + } + + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_NORMAL; + } } bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ - if (!_mission_item_triplet.current_valid) { + if (!_mission_item_valid) { return false; } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + if (_vstatus.is_rotary_wing) { + return _vstatus.condition_landed; + + } else { + /* For fw there is currently no landing detector: + * make sure control is not stopped when overshooting the landing waypoint */ + return false; + } } /* XXX TODO count turns */ - if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); if (!_waypoint_position_reached) { float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; @@ -1265,20 +1433,22 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1287,12 +1457,14 @@ Navigator::check_mission_item_reached() } if (!_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1302,20 +1474,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - if (_mission_item_triplet.current.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + + if (_mission_item.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1328,30 +1502,36 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); } if (_mission.current_mission_available()) { - advance_mission(); + set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1361,67 +1541,16 @@ Navigator::on_mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - //item->yaw - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; - -} - -void -Navigator::get_takeoff_item(mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_TAKEOFF; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* append the home position to RTL item */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; - new_mission_item->altitude_is_relative = false; - new_mission_item->loiter_radius = _parameters.loiter_radius; - new_mission_item->acceptance_radius = _parameters.acceptance_radius; - } -} - -void -Navigator::publish_mission_item_triplet() +Navigator::publish_position_setpoint_triplet() { /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { + if (_pos_sp_triplet_pub > 0) { /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } @@ -1436,42 +1565,85 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + break; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + default: break; + } - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; - if (myState == NAV_STATE_READY) { + + switch (myState) { + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1479,18 +1651,28 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - } else { + break; + + case NAV_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; + break; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); @@ -1506,24 +1688,6 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (a.altitude_is_relative == b.altitude_is_relative && - fabs(a.lat - b.lat) < FLT_EPSILON && - fabs(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - a.autocontinue == b.autocontinue) { - return true; - } else { - return false; - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); @@ -1579,8 +1743,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 8df47fc3b6..af1d9d7d58 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @autho Lorenz Meier + * Author: @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +41,7 @@ * * @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin */ #include @@ -55,6 +57,7 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5bf0fba30e..af04bb0bc4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,15 +71,21 @@ #include "position_estimator_inav_params.h" #include "inertial_filter.h" +#define MIN_VALID_W 0.00001f + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s +static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms +static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss +static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz +static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz +static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -95,8 +101,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -115,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("position_estimator_inav already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -135,16 +140,23 @@ int position_estimator_inav_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tposition_estimator_inav is running\n"); + warnx("app is running"); } else { - printf("\tposition_estimator_inav not started\n"); + warnx("app not started"); } exit(0); @@ -154,32 +166,92 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { + FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { + char *s = malloc(256); + snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fputs(f, s); + snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fputs(f, s); + free(s); + } + fclose(f); +} + /**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started."); + warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_init_cnt = 0; int baro_init_num = 200; - float baro_alt0 = 0.0f; /* to determine while start up */ + float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted + float surface_offset = 0.0f; // ground level offset from reference altitude + float surface_offset_rate = 0.0f; // surface offset change rate float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; + bool ref_inited = false; + hrt_abstime ref_init_start = 0; + const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + + uint16_t accel_updates = 0; + uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; + + hrt_abstime updates_counter_start = hrt_absolute_time(); + hrt_abstime pub_last = hrt_absolute_time(); + + hrt_abstime t_prev = 0; + + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float corr_baro = 0.0f; // D + float corr_gps[3][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) + }; + float w_gps_xy = 1.0f; + float w_gps_z = 1.0f; + float corr_sonar = 0.0f; + float corr_sonar_filtered = 0.0f; + + float corr_flow[] = { 0.0f, 0.0f }; // N E + float w_flow = 0.0f; + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) + hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) + hrt_abstime xy_src_time = 0; // time of last available position data + + bool gps_valid = false; // GPS is valid + bool sonar_valid = false; // sonar is valid + bool flow_valid = false; // flow is valid + bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); @@ -247,75 +319,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_alt0 += sensor.baro_alt_meter; + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } else { wait_baro = false; - baro_alt0 /= (float) baro_init_cnt; - warnx("init baro: alt = %.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + baro_offset /= (float) baro_init_cnt; + warnx("baro offs: %.2f", baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.z_global = true; } } } } } - bool ref_xy_inited = false; - hrt_abstime ref_xy_init_start = 0; - const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix - - hrt_abstime t_prev = 0; - - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; - uint16_t gps_updates = 0; - uint16_t attitude_updates = 0; - uint16_t flow_updates = 0; - - hrt_abstime updates_counter_start = hrt_absolute_time(); - hrt_abstime pub_last = hrt_absolute_time(); - - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D - float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float baro_corr = 0.0f; // D - float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - }; - float sonar_corr = 0.0f; - float sonar_corr_filtered = 0.0f; - float flow_corr[] = { 0.0f, 0.0f }; // X, Y - - float sonar_prev = 0.0f; - hrt_abstime sonar_time = 0; - /* main loop */ - struct pollfd fds[7] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = actuator_sub, .events = POLLIN }, - { .fd = armed_sub, .events = POLLIN }, + struct pollfd fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - if (!thread_should_exit) { - warnx("main loop started."); - } - while (!thread_should_exit) { - int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -324,40 +350,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) continue; } else if (ret > 0) { + /* act on attitude updates */ + + /* vehicle attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; + + bool updated; + /* parameter update */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ + orb_check(parameter_update_sub, &updated); + + if (updated) { struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); - /* update parameters */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ - if (fds[1].revents & POLLIN) { + orb_check(actuator_sub, &updated); + + if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ - if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - } + orb_check(armed_sub, &updated); - /* vehicle attitude */ - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; + if (updated) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + flag_armed = armed.armed; + baro_offset -= z_est[0]; + corr_baro = 0.0f; + local_pos.ref_alt -= z_est[0]; + local_pos.ref_timestamp = t; + z_est[0] = 0.0f; + alt_avg = 0.0f; + } } /* sensor combined */ - if (fds[4].revents & POLLIN) { + orb_check(sensor_combined_sub, &updated); + + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { - /* correct accel bias, now only for Z */ - sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* correct accel bias */ + sensor.accelerometer_m_s2[0] -= acc_bias[0]; + sensor.accelerometer_m_s2[1] -= acc_bias[1]; + sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { @@ -368,12 +414,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_corr[0] = accel_NED[0] - x_est[2]; - accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + corr_acc[0] = accel_NED[0] - x_est[2]; + corr_acc[1] = accel_NED[1] - y_est[2]; + corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { - memset(accel_corr, 0, sizeof(accel_corr)); + memset(corr_acc, 0, sizeof(corr_acc)); } accel_counter = sensor.accelerometer_counter; @@ -381,180 +427,352 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter != baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; + corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } } /* optical flow */ - if (fds[5].revents & POLLIN) { + orb_check(optical_flow_sub, &updated); + + if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { - if (flow.ground_distance_m != sonar_prev) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - sonar_corr = -flow.ground_distance_m - z_est[0]; - sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; + corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; - if (fabsf(sonar_corr) > params.sonar_err) { - // correction is too large: spike or new ground level? - if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { - // spike detected, ignore - sonar_corr = 0.0f; + if (fabsf(corr_sonar) > params.sonar_err) { + /* correction is too large: spike or new ground level? */ + if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { + /* spike detected, ignore */ + corr_sonar = 0.0f; + sonar_valid = false; - } else { - // new ground level - baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - z_est[0] += sonar_corr; - sonar_corr = 0.0f; - sonar_corr_filtered = 0.0f; - } + } else { + /* new ground level */ + surface_offset -= corr_sonar; + surface_offset_rate = 0.0f; + corr_sonar = 0.0f; + corr_sonar_filtered = 0.0f; + sonar_valid_time = t; + sonar_valid = true; + local_pos.surface_bottom_timestamp = t; + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); + } + + } else { + /* correction is ok, use it */ + sonar_valid_time = t; + sonar_valid = true; + } + } + + float flow_q = flow.quality / 255.0f; + float dist_bottom = - z_est[0] - surface_offset; + + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + /* distance to surface */ + float flow_dist = dist_bottom / att.R[2][2]; + /* check if flow if too large for accurate measurements */ + /* calculate estimated velocity in body frame */ + float body_v_est[2] = { 0.0f, 0.0f }; + + for (int i = 0; i < 2; i++) { + body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; + } + + /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ + flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && + fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; + + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; } } + /* velocity correction */ + corr_flow[0] = flow_v[0] - x_est[1]; + corr_flow[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + + /* if flow is not accurate, reduce weight for it */ + // TODO make this more fuzzy + if (!flow_accurate) + w_flow *= 0.05f; + + flow_valid = true; + } else { - sonar_corr = 0.0f; + w_flow = 0.0f; + flow_valid = false; } flow_updates++; } /* vehicle GPS position */ - if (fds[6].revents & POLLIN) { + orb_check(vehicle_gps_position_sub, &updated); + + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { - /* initialize reference position if needed */ - if (!ref_xy_inited) { - /* require EPH < 10m */ - if (gps.eph_m < 10.0f) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; - - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); - } - } else { - ref_xy_init_start = 0; + if (gps.fix_type >= 3) { + /* hysteresis for GPS quality */ + if (gps_valid) { + if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + gps_valid = false; + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } - } - if (ref_xy_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; - - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + } else { + if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + gps_valid = true; + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } + } else { + gps_valid = false; + } + + if (gps_valid) { + /* initialize reference position if needed */ + if (!ref_inited) { + if (ref_init_start == 0) { + ref_init_start = t; + + } else if (t > ref_init_start + ref_init_delay) { + ref_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_alt = alt + z_est[0]; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + } + } + + if (ref_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + corr_gps[0][0] = gps_proj[0] - x_est[0]; + corr_gps[1][0] = gps_proj[1] - y_est[0]; + corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + corr_gps[0][1] = gps.vel_n_m_s - x_est[1]; + corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; + corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; + + } else { + corr_gps[0][1] = 0.0f; + corr_gps[1][1] = 0.0f; + corr_gps[2][1] = 0.0f; + } + + w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); + w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); + } + } else { /* no GPS lock */ - memset(gps_corr, 0, sizeof(gps_corr)); - ref_xy_init_start = 0; + memset(corr_gps, 0, sizeof(corr_gps)); + ref_init_start = 0; } gps_updates++; } } - /* end of poll return value check */ + /* check for timeout on FLOW topic */ + if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { + flow_valid = false; + sonar_valid = false; + warnx("FLOW timeout"); + mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); + } + + /* check for timeout on GPS topic */ + if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + gps_valid = false; + warnx("GPS timeout"); + mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); + } + + /* check for sonar measurement timeout */ + if (sonar_valid && t > sonar_time + sonar_timeout) { + corr_sonar = 0.0f; + sonar_valid = false; + } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - baro_alt0 -= z_est[0]; - z_est[0] = 0.0f; - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + /* use GPS if it's valid and reference position initialized */ + bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; + bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use flow if it's valid and (accurate or no GPS available) */ + bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); + + /* try to estimate position during some time after position sources lost */ + if (use_gps_xy || use_flow) { + xy_src_time = t; } - /* accel bias correction, now only for Z - * not strictly correct, but stable and works */ - accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + + bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + + if (dist_bottom_valid) { + /* surface distance prediction */ + surface_offset += surface_offset_rate * dt; + + /* surface distance correction */ + if (sonar_valid) { + surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; + surface_offset -= corr_sonar * params.w_z_sonar * dt; + } + } + + float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; + float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; + float w_z_gps_p = params.w_z_gps_p * w_gps_z; + + /* reduce GPS weight if optical flow is good */ + if (use_flow && flow_accurate) { + w_xy_gps_p *= params.w_gps_flow; + w_xy_gps_v *= params.w_gps_flow; + } + + /* baro offset correction */ + if (use_gps_z) { + float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; + baro_offset += offs_corr; + baro_counter += offs_corr; + } + + /* accelerometer bias correction */ + float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + + if (use_gps_xy) { + accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; + accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; + } + + if (use_gps_z) { + accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + } + + if (use_flow) { + accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; + accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; + } + + accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += att.R[j][i] * accel_bias_corr[j]; + } + + acc_bias[i] += c * params.w_acc_bias * dt; + } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - baro_alt0 += sonar_corr * params.w_alt_sonar * dt; - inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); - inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - - bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; - bool flow_valid = false; // TODO implement opt flow - - /* try to estimate xy even if no absolute position source available, - * if using optical flow velocity will be correct in this case */ - bool can_estimate_xy = gps_valid || flow_valid; + inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + thread_should_exit = true; + } + /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); + inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); - if (gps_valid) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (use_flow) { + inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); + inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); + } - if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (use_gps_xy) { + inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); + inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); + + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { + inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); + inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } + + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + thread_should_exit = true; + } } /* detect land */ - alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp = z_est[0] - alt_avg; - alt_disp = alt_disp * alt_disp; + alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp2 = - z_est[0] - alt_avg; + alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; /* get actual thrust output */ float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 && thrust > params.land_thr) { landed = false; landed_time = 0; } } else { - if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { landed_time = t; // land detected first time @@ -593,10 +811,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.timestamp = t; - local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.xy_valid = can_estimate_xy && use_gps_xy; local_pos.v_xy_valid = can_estimate_xy; - local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && use_gps_xy; + local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; @@ -605,6 +823,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; + local_pos.dist_bottom_valid = dist_bottom_valid; + + if (local_pos.dist_bottom_valid) { + local_pos.dist_bottom = -z_est[0] - surface_offset; + local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; + } + + local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -614,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; } /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { @@ -634,19 +856,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (local_pos.v_z_valid) { - global_pos.vz = local_pos.vz; + global_pos.vel_d = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } - flag_armed = armed.armed; } - warnx("exiting."); - mavlink_log_info(mavlink_fd, "[inav] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 4f9ddd009d..e1bbd75a6e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,16 +40,19 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); -PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); @@ -57,15 +60,18 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->w_alt_baro = param_find("INAV_W_ALT_BARO"); - h->w_alt_acc = param_find("INAV_W_ALT_ACC"); - h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); - h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); - h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); - h->w_pos_acc = param_find("INAV_W_POS_ACC"); - h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_z_baro = param_find("INAV_W_Z_BARO"); + h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_acc = param_find("INAV_W_Z_ACC"); + h->w_z_sonar = param_find("INAV_W_Z_SONAR"); + h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); + h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_acc = param_find("INAV_W_XY_ACC"); + h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); + h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); h->land_t = param_find("INAV_LAND_T"); @@ -77,15 +83,18 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->w_alt_baro, &(p->w_alt_baro)); - param_get(h->w_alt_acc, &(p->w_alt_acc)); - param_get(h->w_alt_sonar, &(p->w_alt_sonar)); - param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); - param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); - param_get(h->w_pos_acc, &(p->w_pos_acc)); - param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_z_baro, &(p->w_z_baro)); + param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_acc, &(p->w_z_acc)); + param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); + param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_acc, &(p->w_xy_acc)); + param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); + param_get(h->flow_q_min, &(p->flow_q_min)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); param_get(h->land_t, &(p->land_t)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 61570aea7a..e2be079d35 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,15 +41,18 @@ #include struct position_estimator_inav_params { - float w_alt_baro; - float w_alt_acc; - float w_alt_sonar; - float w_pos_gps_p; - float w_pos_gps_v; - float w_pos_acc; - float w_pos_flow; + float w_z_baro; + float w_z_gps_p; + float w_z_acc; + float w_z_sonar; + float w_xy_gps_p; + float w_xy_gps_v; + float w_xy_acc; + float w_xy_flow; + float w_gps_flow; float w_acc_bias; float flow_k; + float flow_q_min; float sonar_filt; float sonar_err; float land_t; @@ -58,15 +61,18 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t w_alt_baro; - param_t w_alt_acc; - param_t w_alt_sonar; - param_t w_pos_gps_p; - param_t w_pos_gps_v; - param_t w_pos_acc; - param_t w_pos_flow; + param_t w_z_baro; + param_t w_z_gps_p; + param_t w_z_acc; + param_t w_z_sonar; + param_t w_xy_gps_p; + param_t w_xy_gps_v; + param_t w_xy_acc; + param_t w_xy_flow; + param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; + param_t flow_q_min; param_t sonar_filt; param_t sonar_err; param_t land_t; diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a2..2f5908ac5e 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1c..5e2c92bf46 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,20 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d00..60eda23192 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a9..2e79f0ac65 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705f..d4c25911e3 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -177,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); @@ -186,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); @@ -204,24 +231,41 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - usleep(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } -#endif + + /* Start the failsafe led init */ + failsafe_led_init(); /* * Run everything in a tight loop. @@ -249,11 +293,14 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } -#if 0 - /* check for debug activity */ + check_reboot(); + + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); @@ -266,7 +313,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663b..393e0560e8 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) @@ -177,12 +178,13 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications @@ -220,3 +222,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index bfc0337f67..2c437d2c0c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -382,7 +382,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -518,16 +520,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) break; - - // note that we don't set BL_WAIT_MAGIC in - // BKP_DR1 as that is not necessary given the - // timing of the forceupdate command. The - // bootloader on px4io waits for enough time - // anyway, and this method works with older - // bootloader versions (tested with both - // revision 3 and revision 4). - up_systemreset(); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -545,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } @@ -606,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 83bd3026eb..ff2e4af6ea 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -83,7 +83,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -164,8 +168,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356a..4954477402 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -218,11 +218,33 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { sbus_frame_drops++; return false; } + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; + } + /* we have received something we think is a frame */ last_frame_time = frame_time; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6833ec43f4..9bac2958e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * Anton Babushkin * @@ -62,6 +62,7 @@ #include #include +#include #include #include #include @@ -72,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -85,13 +86,13 @@ #include #include +#include #include #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" -#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ @@ -108,13 +109,13 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -122,14 +123,17 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char folder_path[64]; +static char log_dir[32]; /* statistics counters */ -static unsigned long log_bytes_written = 0; static uint64_t start_time = 0; +static unsigned long log_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; +/* GPS time, used for log files naming */ +static uint64_t gps_time = 0; + /* current state of logging */ static bool logging_enabled = false; /* enable logging on start (-e option) */ @@ -138,11 +142,14 @@ static bool log_on_start = false; static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ static useconds_t sleep_delay = 0; +/* use date/time for naming directories and files (-t option) */ +static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; static pthread_t logwriter_pthread = 0; +static pthread_attr_t logwriter_attr; /** * Log buffer writing thread. Open and close file here. @@ -203,14 +210,14 @@ static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. Store folder name in 'log_folder'. + * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logfolder(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -218,11 +225,12 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n"); + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -280,82 +288,112 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } -int create_logfolder() +int create_log_dir() { - /* make folder on sdcard */ - uint16_t folder_number = 1; // start with folder sess001 + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - /* look for the next folder that does not exist */ - while (folder_number <= MAX_NO_LOGFOLDER) { - /* set up folder path: e.g. /fs/microsd/sess001 */ - sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ + if (log_name_timestamp && gps_time != 0) { + /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret == 0) { - /* folder does not exist, success */ - break; + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; + } + + } else { + /* look for the next dir that does not exist */ + while (dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + sprintf(log_dir, "%s/sess%03u", log_root, dir_number); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; + } + + /* dir exists already */ + dir_number++; continue; + } - } else { - warn("failed creating new folder"); + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } - if (folder_number >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); - return -1; - } - + /* print logging path, important to find log file later */ + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { - /* make folder on sdcard */ - uint16_t file_number = 1; // start with file log001 - /* string to hold the path to the log */ - char path_buf[64] = ""; + char log_file_name[16] = ""; + char log_file_path[48] = ""; - int fd = 0; + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ - sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); + } else { + uint16_t file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + + if (!file_exist(log_file_path)) { + break; + } - if (file_exist(path_buf)) { file_number++; - continue; } - fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); - - if (fd == 0) { - warn("opening %s failed", path_buf); + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warnx("all %d possible files exist already", MAX_NO_LOGFILE); + return -1; } - - warnx("logging to: %s.", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); - - return fd; } - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); - return -1; + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + + } else { + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } - return 0; + return fd; } static void *logwriter_thread(void *arg) @@ -363,9 +401,12 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + int log_fd = open_log_file(); - int log_fd = open_logfile(); + if (log_fd < 0) + return; + + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); @@ -443,14 +484,20 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return OK; + return; } void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + /* create log dir if needed */ + if (create_log_dir() != 0) { + mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + errx(1, "error creating log dir"); + } + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -458,30 +505,28 @@ void sdlog2_start_log() log_msgs_skipped = 0; /* initialize log buffer emptying thread */ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); + pthread_attr_init(&logwriter_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&logwriter_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; - // XXX we have to destroy the attr at some point } void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -501,6 +546,7 @@ void sdlog2_stop_log() } logwriter_pthread = 0; + pthread_attr_destroy(&logwriter_attr); sdlog2_status(); } @@ -569,8 +615,8 @@ int write_parameters(int fd) } case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; + param_get(param, &value); + break; default: break; @@ -588,18 +634,25 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + logging_enabled = false; + log_on_start = false; + log_when_armed = false; + log_name_timestamp = false; + + flag_system_armed = false; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -632,49 +685,52 @@ int sdlog2_thread_main(int argc, char *argv[]) log_when_armed = true; break; + case 't': + log_name_timestamp = true; + break; + case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); - } - - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); + gps_time = 0; + + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", folder_path); + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); } free(converter_out); - /* only print logging path, important to find log file later */ - warnx("logging to directory: %s", folder_path); - /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -684,6 +740,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; + struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -693,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -702,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct airspeed_s airspeed; struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; + struct battery_status_s battery; } buf; memset(&buf, 0, sizeof(buf)); @@ -709,6 +767,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -726,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int airspeed_sub; int esc_sub; int global_vel_sp_sub; + int battery_sub; } subs; /* log message buffer: header + body */ @@ -752,6 +812,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; struct log_GVSP_s log_GVSP; + struct log_BATT_s log_BATT; + struct log_DIST_s log_DIST; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -760,9 +822,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of subscriptions */ - const ssize_t fdsc = 19; - /* sanity check variable and index */ + /* number of messages */ + const ssize_t fdsc = 25; + /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -785,6 +847,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- VEHICLE CONTROL MODE --- */ + subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + fds[fdsc_count].fd = subs.control_mode_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -840,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -881,12 +949,18 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- BATTERY --- */ + subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); + fds[fdsc_count].fd = subs.battery_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -909,20 +983,31 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t baro_counter = 0; uint16_t differential_pressure_counter = 0; + /* track changes in distance status */ + bool dist_bottom_present = false; + /* enable logging on start if needed */ - if (log_on_start) + if (log_on_start) { + /* check GPS topic to get GPS time */ + if (log_name_timestamp) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { + gps_time = buf.gps_pos.time_gps_usec; + } + } + sdlog2_start_log(); + } while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -936,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + handle_command(&buf.cmd); handled_topics++; } @@ -951,11 +1037,22 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- GPS POSITION - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + if (log_name_timestamp) { + gps_time = buf.gps_pos.time_gps_usec; + } + + handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // begin from fds[1] again + ifds = 1; // begin from VEHICLE STATUS again pthread_mutex_lock(&logbuffer_mutex); @@ -966,14 +1063,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { - // Don't orb_copy, it's already done few lines above + /* don't orb_copy, it's already done few lines above */ + /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ + orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - // TODO use control_mode topic - //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; - log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -998,6 +1094,8 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } + ifds++; // skip CONTROL MODE, already handled + /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1063,6 +1161,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.gx = buf.att.g_comp[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); } @@ -1123,6 +1224,17 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); + + if (buf.local_pos.dist_bottom_valid) { + dist_bottom_present = true; + } + if (dist_bottom_present) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; + log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; + log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } } /* --- LOCAL POSITION SETPOINT --- */ @@ -1140,29 +1252,26 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); + orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; - log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } @@ -1238,6 +1347,17 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GVSP); } + /* --- BATTERY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery); + log_msg.msg_type = LOG_BATT_MSG; + log_msg.body.log_BATT.voltage = buf.battery.voltage_v; + log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; + log_msg.body.log_BATT.current = buf.battery.current_a; + log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; + LOGBUFFER_WRITE_AND_COUNT(BATT); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ @@ -1261,7 +1381,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1274,8 +1394,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1294,7 +1414,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1302,7 +1422,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1313,7 +1433,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3afaaa2ada..98736dd211 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -57,6 +57,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float gx; + float gy; + float gz; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -148,8 +151,6 @@ struct log_STAT_s { uint8_t main_state; uint8_t navigation_state; uint8_t arming_state; - float battery_voltage; - float battery_current; float battery_remaining; uint8_t battery_warning; uint8_t landed; @@ -209,16 +210,13 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t altitude_is_relative; int32_t lat; int32_t lon; - float altitude; + float alt; float yaw; - uint8_t nav_cmd; + uint8_t type; float loiter_radius; int8_t loiter_direction; - float acceptance_radius; - float time_inside; float pitch_min; }; @@ -247,6 +245,25 @@ struct log_GVSP_s { float vz; }; +/* --- BATT - BATTERY --- */ +#define LOG_BATT_MSG 20 +struct log_BATT_s { + float voltage; + float voltage_filtered; + float current; + float discharged; +}; + +/* --- DIST - DISTANCE TO SURFACE --- */ +#define LOG_DIST_MSG 21 +struct log_DIST_s { + float bottom; + float bottom_rate; + uint8_t flags; +}; + +/********** SYSTEM MESSAGES, ID > 0x80 **********/ + /* --- TIME - TIME STAMP --- */ #define LOG_TIME_MSG 129 struct log_TIME_s { @@ -272,7 +289,7 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), @@ -280,16 +297,18 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), + LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index ebbc580e10..aa538fd6b1 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/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 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7637235548..30659fd3a0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * 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 @@ -38,6 +35,10 @@ * @file sensor_params.c * * Parameters defined by the sensors task. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include @@ -45,41 +46,98 @@ #include /** - * Gyro X offset FIXME + * Gyro X offset * - * This is an X-axis offset for the gyro. - * Adjust it according to the calibration data. + * This is an X-axis offset for the gyro. Adjust it according to the calibration data. * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset FIXME with dot. + * Gyro Y offset * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset FIXME + * Gyro Z offset * * @min -5.0 * @max 5.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +/** + * Gyro X scaling + * + * X-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); + +/** + * Gyro Y scaling + * + * Y-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); + +/** + * Gyro Z scaling + * + * Z-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * Magnetometer X offset + * + * This is an X-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); + +/** + * Magnetometer Y offset + * + * This is an Y-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); + +/** + * Magnetometer Z offset + * + * This is an Z-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); @@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** + * RC Channel 1 Minimum + * + * Minimum value for RC channel 1 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); + +/** + * RC Channel 1 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); + +/** + * RC Channel 1 Maximum + * + * Maximum value for RC channel 1 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); + +/** + * RC Channel 1 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +/** + * RC Channel 1 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); -PARAM_DEFINE_FLOAT(RC2_MIN, 1000); -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +/** + * RC Channel 2 Minimum + * + * Minimum value for RC channel 2 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); + +/** + * RC Channel 2 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC Channel 2 Maximum + * + * Maximum value for RC channel 2 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC Channel 2 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC Channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); @@ -223,15 +379,75 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9baf1a6aff..ea864390da 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -114,6 +114,7 @@ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #endif @@ -124,10 +125,8 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif -#define BAT_VOL_INITIAL 0.f -#define BAT_VOL_LOWPASS_1 0.99f -#define BAT_VOL_LOWPASS_2 0.01f -#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f +#define BATT_V_LOWPASS 0.001f +#define BATT_V_IGNORE_THRESHOLD 3.5f /** * HACK - true temperature is much less than indicated temperature in baro, @@ -211,10 +210,13 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ + hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -265,6 +267,7 @@ private: float rc_fs_thr; float battery_voltage_scaling; + float battery_current_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -314,6 +317,7 @@ private: param_t rc_fs_thr; param_t battery_voltage_scaling; + param_t battery_current_scaling; param_t board_rotation; param_t external_mag_rotation; @@ -465,9 +469,9 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), - _mag_is_external(false) + _mag_is_external(false), + _battery_discharged(0), + _battery_current_timestamp(0) { /* basic r/c parameters */ @@ -560,6 +564,7 @@ Sensors::Sensors() : _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -740,6 +745,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* scaling of ADC ticks to battery current */ + if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { + warnx("Failed updating current scaling param"); + } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); @@ -785,7 +795,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -825,7 +834,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -920,7 +928,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +954,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -972,7 +980,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; @@ -1157,17 +1165,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (!_publishing) return; + hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ - if (hrt_absolute_time() - _last_adc >= 10000) { + if (t - _last_adc >= 10000) { /* make space for a maximum of eight channels */ struct adc_msg_s buf_adc[8]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - - if (ret >= (int)sizeof(buf_adc[0])) { - + if (ret >= (int)sizeof(buf_adc[0])) { + for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); @@ -1178,27 +1185,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); - if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - + if (voltage > BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_v = voltage; /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_v < 3.0f) { - _battery_status.voltage_v = voltage; + if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_filtered_v = voltage; } - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; + _battery_status.timestamp = t; + _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; - /* announce the battery voltage if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + } else { + /* mark status as invalid */ + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + } - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { + /* handle current only if voltage is valid */ + if (_battery_status.voltage_v > 0.0f) { + float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ + if (current >= 0.0f) { + _battery_status.timestamp = t; + _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { + /* initialize discharged value */ + if (_battery_status.discharged_mah < 0.0f) + _battery_status.discharged_mah = 0.0f; + _battery_discharged += current * (t - _battery_current_timestamp); + _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; + } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1214,7 +1234,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor - _diff_pres.timestamp = hrt_absolute_time(); + _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.voltage = voltage; @@ -1227,8 +1247,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } + } + _last_adc = t; + if (_battery_status.voltage_v > 0.0f) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - _last_adc = hrt_absolute_time(); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } } } @@ -1475,9 +1503,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); @@ -1516,7 +1541,10 @@ Sensors::task_main() raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); - _battery_status.voltage_v = BAT_VOL_INITIAL; + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; /* get a set of initial values */ accel_poll(raw); diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/modules/systemlib/board_serial.c similarity index 65% rename from src/lib/mathlib/math/Vector3.hpp rename to src/modules/systemlib/board_serial.c index 568d9669ab..ad8c2bf83e 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/modules/systemlib/board_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -32,45 +32,29 @@ ****************************************************************************/ /** - * @file Vector3.hpp + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt * - * math 3 vector */ -#pragma once +#include "otp.h" +#include "board_config.h" +#include "board_serial.h" -#include "Vector.hpp" - -namespace math +int get_board_serial(char *serialid) { + const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + union udid id; + val_read((unsigned *)&id, udid_ptr, sizeof(id)); -class __EXPORT Vector3 : - public Vector -{ -public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ -}; - -int __EXPORT vector3Test(); -} // math + /* Copy the serial from the chips non-write memory and swap endianess */ + serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0]; + serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4]; + serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; + return 0; +} \ No newline at end of file diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/modules/systemlib/board_serial.h similarity index 84% rename from src/lib/mathlib/math/arm/Matrix.cpp rename to src/modules/systemlib/board_serial.h index 21661622ab..b14bb4376b 100644 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ b/src/modules/systemlib/board_serial.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -32,9 +32,18 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt * - * matrix code */ -#include "Matrix.hpp" +#pragma once + +__BEGIN_DECLS + +__EXPORT int get_board_serial(char *serialid); + +__END_DECLS diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 8aca6a25dd..49403c98bd 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder) memcpy(encoder->buf, &len, sizeof(len)); } + /* sync file */ + fsync(encoder->fd); + return 0; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 843cda7225..3953b757d2 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -49,4 +49,8 @@ SRCS = err.c \ airspeed.c \ system_params.c \ mavlink_log.c \ - rc_check.c + rc_check.c \ + otp.c \ + board_serial.c \ + pwm_limit/pwm_limit.c + diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c new file mode 100644 index 0000000000..695574fdcb --- /dev/null +++ b/src/modules/systemlib/otp.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Authors: + * Lorenz Meier + * David "Buzz" Bussenschutt + * + * 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 otp.c + * otp estimation + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#include +#include +#include +#include +#include +#include // memset +#include "conversions.h" +#include "otp.h" +#include "err.h" // warnx +#include + + +int val_read(void *dest, volatile const void *src, int bytes) +{ + + int i; + + for (i = 0; i < bytes / 4; i++) { + *(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i); + } + + return i * 4; +} + + +int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) +{ + + warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid); + + int errors = 0; + + // descriptor + if (F_write_byte(ADDR_OTP_START, 'P')) + errors++; + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) + errors++; // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 2, '4')) + errors++; + if (F_write_byte(ADDR_OTP_START + 3, '\0')) + errors++; + //id_type + if (F_write_byte(ADDR_OTP_START + 4, id_type)) + errors++; + // vid and pid are 4 bytes each + if (F_write_word(ADDR_OTP_START + 5, vid)) + errors++; + if (F_write_word(ADDR_OTP_START + 9, pid)) + errors++; + + // leave some 19 bytes of space, and go to the next block... + // then the auth sig starts + for (int i = 0 ; i < 128 ; i++) { + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + errors++; + } + + return errors; +} + +int lock_otp(void) +{ + //determine the required locking size - can only write full lock bytes */ +// int size = sizeof(struct otp) / 32; +// +// struct otp_lock otp_lock_mem; +// +// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); +// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) +// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; + //XXX add the actual call here to write the OTP_LOCK bytes only at final stage + // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); + + int locksize = 5; + + int errors = 0; + + // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. + for (int i = 0 ; i < locksize ; i++) { + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + errors++; + } + + return errors; +} + + + +// COMPLETE, BUSY, or other flash error? +int F_GetStatus(void) +{ + int fs = F_COMPLETE; + + if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { + + if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { + + if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { + + if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { + fs = F_COMPLETE; + } + } + } + } + + return fs; +} + + +// enable FLASH Registers +void F_unlock(void) +{ + if ((FLASH->control & F_CR_LOCK) != 0) { + FLASH->key = F_KEY1; + FLASH->key = F_KEY2; + } +} + +// lock the FLASH Registers +void F_lock(void) +{ + FLASH->control |= F_CR_LOCK; +} + +// flash write word. +int F_write_word(uint32_t Address, uint32_t Data) +{ + unsigned char octet[4] = {0, 0, 0, 0}; + + int ret = 0; + + for (int i = 0; i < 4; i++) { + octet[i] = (Data >> (i * 8)) & 0xFF; + ret = F_write_byte(Address + i, octet[i]); + } + + return ret; +} + +// flash write byte +int F_write_byte(uint32_t Address, uint8_t Data) +{ + volatile int status = F_COMPLETE; + + //warnx("F_write_byte: %08X %02d", Address , Data ) ; + + //Check the parameters + assert(IS_F_ADDRESS(Address)); + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + if (status == F_COMPLETE) { + //if the previous operation is completed, proceed to program the new data + FLASH->control &= CR_PSIZE_MASK; + FLASH->control |= F_PSIZE_BYTE; + FLASH->control |= F_CR_PG; + + *(volatile uint8_t *)Address = Data; + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + //if the program operation is completed, disable the PG Bit + FLASH->control &= (~F_CR_PG); + } + + //Return the Program Status + return !(status == F_COMPLETE); +} + + + diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h new file mode 100644 index 0000000000..f10e129d8c --- /dev/null +++ b/src/modules/systemlib/otp.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 otp.h + * One TIme Programmable ( OTP ) Flash routine/s. + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#ifndef OTP_H_ +#define OTP_H_ + +__BEGIN_DECLS + +#define ADDR_OTP_START 0x1FFF7800 +#define ADDR_OTP_LOCK_START 0x1FFF7A00 + +#define OTP_LOCK_LOCKED 0x00 +#define OTP_LOCK_UNLOCKED 0xFF + + + +#include +#include + +// possible flash statuses +#define F_BUSY 1 +#define F_ERROR_WRP 2 +#define F_ERROR_PROGRAM 3 +#define F_ERROR_OPERATION 4 +#define F_COMPLETE 5 + +typedef struct { + volatile uint32_t accesscontrol; // 0x00 + volatile uint32_t key; // 0x04 + volatile uint32_t optionkey; // 0x08 + volatile uint32_t status; // 0x0C + volatile uint32_t control; // 0x10 + volatile uint32_t optioncontrol; //0x14 +} flash_registers; + +#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) +#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) +#define FLASH ((flash_registers *) F_R_BASE) + +#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) +#define F_PSIZE_WORD ((uint32_t)0x00000200) +#define F_PSIZE_BYTE ((uint32_t)0x00000000) +#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. + +#define F_KEY1 ((uint32_t)0x45670123) +#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) + + + +#pragma pack(push, 1) + +/* + * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes. + * The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15) + * to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed + * until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only + * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly. + */ + +struct otp { + // first 32 bytes = the '0' Block + char id[4]; ///4 bytes < 'P' 'X' '4' '\n' + uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID + uint32_t vid; ///4 bytes + uint32_t pid; ///4 bytes + char unused[19]; ///19 bytes + // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) + char signature[128]; + // insert extras here + uint32_t lock_bytes[4]; +}; + +struct otp_lock { + uint8_t lock_bytes[16]; +}; +#pragma pack(pop) + +#define ADDR_F_SIZE 0x1FFF7A22 + +#pragma pack(push, 1) +union udid { + uint32_t serial[3]; + char data[12]; +}; +#pragma pack(pop) + + +/** + * s + */ +//__EXPORT float calc_indicated_airspeed(float differential_pressure); + +__EXPORT void F_unlock(void); +__EXPORT void F_lock(void); +__EXPORT int val_read(void *dest, volatile const void *src, int bytes); +__EXPORT int val_write(volatile void *dest, const void *src, int bytes); +__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature); +__EXPORT int lock_otp(void); + + +__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); +__EXPORT int F_write_word(uint32_t Address, uint32_t Data); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 398657dd79..2d773fd251 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -61,7 +61,7 @@ #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" -#if 1 +#if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else # define debug(fmt, args...) do { } while(0) @@ -512,6 +512,28 @@ param_save_default(void) int fd; const char *filename = param_get_default_file(); + + /* write parameters to temp file */ + fd = open(filename, O_WRONLY | O_CREAT); + + if (fd < 0) { + warn("failed to open param file: %s", filename); + return ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); + } + } + + close(fd); + + return res; + +#if 0 const char *filename_tmp = malloc(strlen(filename) + 5); sprintf(filename_tmp, "%s.tmp", filename); @@ -565,6 +587,7 @@ param_save_default(void) free(filename_tmp); return res; +#endif } /** @@ -573,9 +596,9 @@ param_save_default(void) int param_load_default(void) { - int fd = open(param_get_default_file(), O_RDONLY); + int fd_load = open(param_get_default_file(), O_RDONLY); - if (fd < 0) { + if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { warn("open '%s' for reading failed", param_get_default_file()); @@ -584,8 +607,8 @@ param_load_default(void) return 1; } - int result = param_load(fd); - close(fd); + int result = param_load(fd_load); + close(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 77c952f52c..6a4e9392a2 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -39,7 +39,7 @@ /** * @file pid.c * - * Implementation of generic PID control interface. + * Implementation of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -53,24 +53,21 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->limit = limit; pid->mode = mode; pid->dt_min = dt_min; - pid->count = 0.0f; - pid->saturated = 0.0f; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) + +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) { int ret = 0; @@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(intmax)) { - pid->intmax = intmax; + if (isfinite(integral_limit)) { + pid->integral_limit = integral_limit; } else { ret = 1; } - if (isfinite(limit)) { - pid->limit = limit; + if (isfinite(output_limit)) { + pid->output_limit = output_limit; } else { ret = 1; @@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float return ret; } -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* current error value */ + float error = sp - val; - // Calculate or measured current error derivative + /* current error derivative */ if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); pid->error_previous = error; @@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - if (pid->ki > 0.0f) { + /* calculate PD output */ + float output = (error * pid->kp) + (d * pid->kd); + + if (pid->ki > SIGMA) { // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; - - } else { - if (!isfinite(i)) { - i = 0.0f; + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ + pid->integral = i; } - - pid->integral = i; - pid->saturated = 0; } - } else { - i = 0.0f; - pid->saturated = 0; + /* add I component to output */ + output += pid->integral * pid->ki; } - // Calculate the output. Limit output magnitude to pid->limit - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - + /* limit output */ if (isfinite(output)) { - if (pid->limit > SIGMA) { - if (output > pid->limit) { - output = pid->limit; + if (pid->output_limit > SIGMA) { + if (output > pid->output_limit) { + output = pid->output_limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; } } @@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo __EXPORT void pid_reset_integral(PID_t *pid) { - pid->integral = 0; + pid->integral = 0.0f; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eca228464f..e8b1aac4fd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -39,7 +39,7 @@ /** * @file pid.h * - * Definition of generic PID control interface. + * Definition of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -55,38 +55,35 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC_NO_SP 1 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 2 -// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) -#define PID_MODE_DERIVATIV_NONE 9 +typedef enum PID_MODE { + /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ + PID_MODE_DERIVATIV_NONE = 0, + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, + * val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC, + /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, + * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC_NO_SP, + /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ + PID_MODE_DERIVATIV_SET +} pid_mode_t; typedef struct { + pid_mode_t mode; + float dt_min; float kp; float ki; float kd; - float intmax; - float sp; float integral; + float integral_limit; + float output_limit; float error_previous; float last_output; - float limit; - float dt_min; - uint8_t mode; - uint8_t count; - uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); -//void pid_set(PID_t *pid, float sp); +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - __EXPORT void pid_reset_integral(PID_t *pid); __END_DECLS diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cac3dc82a3..190b315f1f 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without @@ -44,38 +44,53 @@ #include #include #include +#include void pwm_limit_init(pwm_limit_t *limit) { - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_INIT; limit->time_armed = 0; return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { + /* first evaluate state changes */ switch (limit->state) { - case LIMIT_STATE_OFF: - if (armed) - limit->state = LIMIT_STATE_RAMP; - limit->time_armed = hrt_absolute_time(); + case PWM_LIMIT_STATE_INIT: + + if (armed) { + + /* set arming time for the first call */ + if (limit->time_armed == 0) { + limit->time_armed = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { + limit->state = PWM_LIMIT_STATE_OFF; + } + } break; - case LIMIT_STATE_INIT: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) - limit->state = LIMIT_STATE_RAMP; + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } break; - case LIMIT_STATE_RAMP: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) - limit->state = LIMIT_STATE_ON; + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } break; - case LIMIT_STATE_ON: - if (!armed) - limit->state = LIMIT_STATE_OFF; + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } break; default: break; @@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ /* then set effective_pwm based on state */ switch (limit->state) { - case LIMIT_STATE_OFF: - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: for (unsigned i=0; itime_armed); - progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; i 0) { + progress = diff * 10000 / RAMP_TIME_US; - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; + for (unsigned i=0; i 0) { - uint16_t disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; } - - effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - output[i] = (float)progress/10000.0f * output[i]; } break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: for (unsigned i=0; i #include +__BEGIN_DECLS + /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -56,21 +58,21 @@ */ #define RAMP_TIME_US 2500000 +enum pwm_limit_state { + PWM_LIMIT_STATE_OFF = 0, + PWM_LIMIT_STATE_INIT, + PWM_LIMIT_STATE_RAMP, + PWM_LIMIT_STATE_ON +}; + typedef struct { - enum { - LIMIT_STATE_OFF = 0, - LIMIT_STATE_INIT, - LIMIT_STATE_RAMP, - LIMIT_STATE_ON - } state; + enum pwm_limit_state state; uint64_t time_armed; } pwm_limit_t; -__BEGIN_DECLS - __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 79a820c066..4c84c1f25e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/mission_item_triplet.h" -ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); +#include "topics/position_setpoint_triplet.h" +ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h index c40d0d4e56..d473dff3f4 100644 --- a/src/modules/uORB/topics/battery_status.h +++ b/src/modules/uORB/topics/battery_status.h @@ -53,9 +53,10 @@ */ struct battery_status_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float voltage_v; /**< Battery voltage in volts, filtered */ - float current_a; /**< Battery current in amperes, filtered, -1 if unknown */ - float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */ + float voltage_v; /**< Battery voltage in volts, 0 if unknown */ + float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */ + float current_a; /**< Battery current in amperes, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */ }; /** @@ -65,4 +66,4 @@ struct battery_status_s { /* register this as object request broker structure */ ORB_DECLARE(battery_status); -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e9..08d11abaea 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h similarity index 72% rename from src/modules/uORB/topics/mission_item_triplet.h rename to src/modules/uORB/topics/position_setpoint_triplet.h index b35eae6070..4b57833b63 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -46,31 +46,42 @@ #include #include "../uORB.h" -#include "mission.h" - /** * @addtogroup topics * @{ */ +enum SETPOINT_TYPE +{ + SETPOINT_TYPE_NORMAL = 0, + SETPOINT_TYPE_LOITER, + SETPOINT_TYPE_TAKEOFF, + SETPOINT_TYPE_LAND, +}; + +struct position_setpoint_s +{ + bool valid; /**< true if setpoint is valid */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + double lat; /**< latitude, in deg */ + double lon; /**< longitude, in deg */ + float alt; /**< altitude AMSL, in m */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float loiter_radius; /**< loiter radius (only for fixed wing), in m */ + int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ +}; + /** * Global position setpoint triplet in WGS84 coordinates. * * This are the three next waypoints (or just the next two or one). */ -struct mission_item_triplet_s +struct position_setpoint_triplet_s { - bool previous_valid; - bool current_valid; /**< flag indicating previous mission item is valid */ - bool next_valid; /**< flag indicating next mission item is valid */ - - struct mission_item_s previous; - struct mission_item_s current; - struct mission_item_s next; - - int previous_index; - int current_index; - int next_index; + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; }; /** @@ -78,6 +89,6 @@ struct mission_item_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); +ORB_DECLARE(position_setpoint_triplet); #endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 4380a5ee77..e5a35ff9bc 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,6 +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 */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a8..7596f944f3 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -61,7 +61,7 @@ struct vehicle_attitude_setpoint_s float yaw_body; /**< body angle in NED frame */ //float body_valid; /**< Set to true if body angles are valid */ - float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ //! For quaternion-based attitude control diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985c..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; @@ -92,7 +93,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e372..ae771ca008 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,7 +54,7 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. @@ -65,14 +65,13 @@ struct vehicle_global_position_s uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ - float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 4271537829..d567f2e020 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,6 +77,11 @@ struct vehicle_local_position_s int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ + /* Distance to surface */ + float dist_bottom; /**< Distance to bottom surface (ground) */ + float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ + uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ + bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f51..a5988d3ba4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { @@ -82,9 +84,12 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +178,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +229,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 2aed80e011..0000000000 --- a/src/systemcmds/eeprom/eeprom.c +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 eeprom.c - * - * EEPROM service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -#ifndef PX4_I2C_BUS_ONBOARD -# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM -#endif - -__EXPORT int eeprom_main(int argc, char *argv[]); - -static void eeprom_attach(void); -static void eeprom_start(void); -static void eeprom_erase(void); -static void eeprom_ioctl(unsigned operation); -static void eeprom_save(const char *name); -static void eeprom_load(const char *name); -static void eeprom_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *eeprom_mtd; - -int eeprom_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - eeprom_start(); - - if (!strcmp(argv[1], "save_param")) - eeprom_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - eeprom_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - eeprom_erase(); - - if (!strcmp(argv[1], "test")) - eeprom_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - eeprom_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - eeprom_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); -} - - -static void -eeprom_attach(void) -{ - /* find the right I2C */ - struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - /* this resets the I2C bus, set correct bus speed again */ - I2C_SETFREQUENCY(i2c, 400000); - - if (i2c == NULL) - errx(1, "failed to locate I2C bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - eeprom_mtd = at24c_initialize(i2c); - if (eeprom_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (eeprom_mtd == NULL) - errx(1, "failed to initialize EEPROM driver"); - - attached = true; -} - -static void -eeprom_start(void) -{ - int ret; - - if (started) - errx(1, "EEPROM already mounted"); - - if (!attached) - eeprom_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(eeprom_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - - /* mount the EEPROM */ - ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - - started = true; - warnx("mounted EEPROM at /eeprom"); - exit(0); -} - -extern int at24c_nuke(void); - -static void -eeprom_erase(void) -{ - if (!attached) - eeprom_attach(); - - if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -eeprom_ioctl(unsigned operation) -{ - int fd; - - fd = open("/eeprom/.", 0); - - if (fd < 0) - err(1, "open /eeprom"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -eeprom_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -eeprom_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -extern void at24c_test(void); - -static void -eeprom_test(void) -{ - at24c_test(); - exit(0); -} diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/systemcmds/hw_ver/hw_ver.c similarity index 69% rename from src/lib/mathlib/math/generic/Matrix.cpp rename to src/systemcmds/hw_ver/hw_ver.c index 21661622ab..4b84523cc1 100644 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ b/src/systemcmds/hw_ver/hw_ver.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -32,9 +32,42 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file hw_ver.c * - * matrix code + * Show and test hardware version. */ -#include "Matrix.hpp" +#include + +#include +#include +#include +#include + +__EXPORT int hw_ver_main(int argc, char *argv[]); + +int +hw_ver_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "show")) { + printf(HW_ARCH "\n"); + exit(0); + } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 3) { + int ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + exit(ret); + + } else { + errx(1, "not enough arguments, try 'compare PX4FMU_1'"); + } + } + } + + errx(1, "expected a command, try 'show' or 'compare'"); +} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk new file mode 100644 index 0000000000..3cc08b6a1a --- /dev/null +++ b/src/systemcmds/hw_ver/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = hw_ver +SRCS = hw_ver.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c similarity index 100% rename from src/systemcmds/eeprom/24xxxx_mtd.c rename to src/systemcmds/mtd/24xxxx_mtd.c diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk new file mode 100644 index 0000000000..b3fceceb5c --- /dev/null +++ b/src/systemcmds/mtd/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = mtd +SRCS = mtd.c 24xxxx_mtd.c diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c new file mode 100644 index 0000000000..a2a0c109c6 --- /dev/null +++ b/src/systemcmds/mtd/mtd.c @@ -0,0 +1,493 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 mtd.c + * + * mtd service and utility app. + * + * @author Lorenz Meier + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +#include + +__EXPORT int mtd_main(int argc, char *argv[]); + +#ifndef CONFIG_MTD + +/* create a fake command with decent warnx to not confuse users */ +int mtd_main(int argc, char *argv[]) +{ + errx(1, "MTD not enabled, skipping."); +} + +#else + +#ifdef CONFIG_MTD_RAMTRON +static void ramtron_attach(void); +#else + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM +#endif + +static void at24xxx_attach(void); +#endif +static void mtd_start(char *partition_names[], unsigned n_partitions); +static void mtd_test(void); +static void mtd_erase(char *partition_names[], unsigned n_partitions); +static void mtd_readtest(char *partition_names[], unsigned n_partitions); +static void mtd_rwtest(char *partition_names[], unsigned n_partitions); +static void mtd_print_info(); +static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *mtd_dev; +static unsigned n_partitions_current = 0; + +/* note, these will be equally sized */ +static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; +static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); + +int mtd_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) { + + /* start mapping according to user request */ + if (argc >= 3) { + mtd_start(argv + 2, argc - 2); + } else { + mtd_start(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "test")) + mtd_test(); + + if (!strcmp(argv[1], "readtest")) { + if (argc >= 3) { + mtd_readtest(argv + 2, argc - 2); + } else { + mtd_readtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "rwtest")) { + if (argc >= 3) { + mtd_rwtest(argv + 2, argc - 2); + } else { + mtd_rwtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "status")) + mtd_status(); + + if (!strcmp(argv[1], "erase")) { + if (argc >= 3) { + mtd_erase(argv + 2, argc - 2); + } else { + mtd_erase(partition_names_default, n_partitions_default); + } + } + } + + errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); +struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, + off_t firstblock, off_t nblocks); + +#ifdef CONFIG_MTD_RAMTRON +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the RAMTRON driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = ramtron_initialize(spi); + + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: mtd needed %d attempts to attach", i + 1); + } + + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize mtd driver"); + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); + if (ret != OK) + warnx(1, "failed to set bus speed"); + + attached = true; +} +#else + +static void +at24xxx_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = at24c_initialize(i2c); + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} +#endif + +static void +mtd_start(char *partition_names[], unsigned n_partitions) +{ + int ret; + + if (started) + errx(1, "mtd already mounted"); + + if (!attached) { + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + at24xxx_attach(); + #else + ramtron_attach(); + #endif + } + + if (!mtd_dev) { + warnx("ERROR: Failed to create RAMTRON FRAM MTD instance"); + exit(1); + } + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions); + if (ret) + exit(3); + + /* Now create MTD FLASH partitions */ + + warnx("Creating partitions"); + FAR struct mtd_dev_s *part[n_partitions]; + char blockname[32]; + + unsigned offset; + unsigned i; + + for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) { + + warnx(" Partition %d. Block offset=%lu, size=%lu", + i, (unsigned long)offset, (unsigned long)nblocks); + + /* Create the partition */ + + part[i] = mtd_partition(mtd_dev, offset, nblocks); + + if (!part[i]) { + warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu", + (unsigned long)offset, (unsigned long)nblocks); + exit(4); + } + + /* Initialize to provide an FTL block driver on the MTD FLASH interface */ + + snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i); + + ret = ftl_initialize(i, part[i]); + + if (ret < 0) { + warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret); + exit(5); + } + + /* Now create a character device on the block device */ + + ret = bchdev_register(blockname, partition_names[i], false); + + if (ret < 0) { + warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret); + exit(6); + } + } + + n_partitions_current = n_partitions; + + started = true; + exit(0); +} + +int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) +{ + /* Get the geometry of the FLASH device */ + + FAR struct mtd_geometry_s geo; + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + + if (ret < 0) { + warnx("ERROR: mtd->ioctl failed: %d", ret); + return ret; + } + + *blocksize = geo.blocksize; + *erasesize = geo.blocksize; + *neraseblocks = geo.neraseblocks; + + /* Determine the size of each partition. Make each partition an even + * multiple of the erase block size (perhaps not using some space at the + * end of the FLASH). + */ + + *blkpererase = geo.erasesize / geo.blocksize; + *nblocks = (geo.neraseblocks / n_partitions) * *blkpererase; + *partsize = *nblocks * geo.blocksize; + + return ret; +} + +/* + get partition size in bytes + */ +static ssize_t mtd_get_partition_size(void) +{ + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize = 0; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret != OK) { + errx(1, "Failed to get geometry"); + } + return partsize; +} + +void mtd_print_info() +{ + if (!attached) + exit(1); + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret) + exit(3); + + warnx("Flash Geometry:"); + + printf(" blocksize: %lu\n", blocksize); + printf(" erasesize: %lu\n", erasesize); + printf(" neraseblocks: %lu\n", neraseblocks); + printf(" No. partitions: %u\n", n_partitions_current); + printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize); + printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024); + +} + +void +mtd_test(void) +{ + warnx("This test routine does not test anything yet!"); + exit(1); +} + +void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + +void +mtd_erase(char *partition_names[], unsigned n_partitions) +{ + uint8_t v[64]; + memset(v, 0xFF, sizeof(v)); + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("Erasing %s\n", partition_names[i]); + int fd = open(partition_names[i], O_WRONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (write(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + printf("Erased %lu bytes\n", (unsigned long)count); + close(fd); + } + exit(0); +} + +/* + readtest is useful during startup to validate the device is + responding on the bus. It relies on the driver returning an error on + bad reads (the ramtron driver does return an error) + */ +void +mtd_readtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("readtest OK\n"); + exit(0); +} + +/* + rwtest is useful during startup to validate the device is + responding on the bus for both reads and writes. It reads data in + blocks and writes the data back, then reads it again, failing if the + data isn't the same + */ +void +mtd_rwtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128], v2[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + off_t offset = 0; + printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDWR); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (write(fd, v, sizeof(v)) != sizeof(v)) { + errx(1, "write failed"); + } + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { + errx(1, "read failed"); + } + if (memcmp(v, v2, sizeof(v2)) != 0) { + errx(1, "memcmp failed"); + } + offset += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("rwtest OK\n"); + exit(0); +} + +#endif diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 458bb2259e..7d9484d3eb 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,9 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 50) { + + /* try the first 30 seconds */ + while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 65f291f401..580fdc62f4 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -72,7 +72,12 @@ param_main(int argc, char *argv[]) if (argc >= 3) { do_save(argv[2]); } else { - do_save(param_get_default_file()); + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } } } @@ -133,11 +138,8 @@ param_main(int argc, char *argv[]) static void do_save(const char* param_file_name) { - /* delete the parameter file in case it exists */ - unlink(param_file_name); - /* create the file */ - int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL); + int fd = open(param_file_name, O_WRONLY | O_CREAT); if (fd < 0) err(1, "opening '%s' failed", param_file_name); @@ -146,7 +148,7 @@ do_save(const char* param_file_name) close(fd); if (result < 0) { - unlink(param_file_name); + (void)unlink(param_file_name); errx(1, "error exporting to '%s'", param_file_name); } @@ -203,11 +205,38 @@ do_show_print(void *arg, param_t param) int32_t i; float f; const char *search_string = (const char*)arg; + const char *p_name = (const char*)param_name(param); /* print nothing if search string is invalid and not matching */ - if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { - /* param not found */ - return; + if (!(arg == NULL)) { + + /* start search */ + char *ss = search_string; + char *pp = p_name; + bool mismatch = false; + + /* XXX this comparison is only ok for trailing wildcards */ + while (*ss != '\0' && *pp != '\0') { + + if (*ss == *pp) { + ss++; + pp++; + } else if (*ss == '*') { + if (*(ss + 1) != '\0') { + warnx("* symbol only allowed at end of search string."); + exit(1); + } + + pp++; + } else { + /* param not found */ + return; + } + } + + /* the search string must have been consumed */ + if (!(*ss == '\0' || *ss == '*')) + return; } printf("%c %s: ", diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk deleted file mode 100644 index e4eb1d143c..0000000000 --- a/src/systemcmds/ramtron/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# RAMTRON file system driver -# - -MODULE_COMMAND = ramtron -SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c deleted file mode 100644 index 03c713987a..0000000000 --- a/src/systemcmds/ramtron/ramtron.c +++ /dev/null @@ -1,279 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 ramtron.c - * - * ramtron service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -__EXPORT int ramtron_main(int argc, char *argv[]); - -#ifndef CONFIG_MTD_RAMTRON - -/* create a fake command with decent message to not confuse users */ -int ramtron_main(int argc, char *argv[]) -{ - errx(1, "RAMTRON not enabled, skipping."); -} -#else - -static void ramtron_attach(void); -static void ramtron_start(void); -static void ramtron_erase(void); -static void ramtron_ioctl(unsigned operation); -static void ramtron_save(const char *name); -static void ramtron_load(const char *name); -static void ramtron_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *ramtron_mtd; - -int ramtron_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - ramtron_start(); - - if (!strcmp(argv[1], "save_param")) - ramtron_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - ramtron_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - ramtron_erase(); - - if (!strcmp(argv[1], "test")) - ramtron_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - ramtron_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - ramtron_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); -} - -struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); - - -static void -ramtron_attach(void) -{ - /* find the right spi */ - struct spi_dev_s *spi = up_spiinitialize(2); - /* this resets the spi bus, set correct bus speed again */ - // xxx set in ramtron driver, leave this out -// SPI_SETFREQUENCY(spi, 4000000); - SPI_SETFREQUENCY(spi, 375000000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SELECT(spi, SPIDEV_FLASH, false); - - if (spi == NULL) - errx(1, "failed to locate spi bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - ramtron_mtd = ramtron_initialize(spi); - if (ramtron_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: ramtron needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (ramtron_mtd == NULL) - errx(1, "failed to initialize ramtron driver"); - - attached = true; -} - -static void -ramtron_start(void) -{ - int ret; - - if (started) - errx(1, "ramtron already mounted"); - - if (!attached) - ramtron_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(ramtron_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); - - /* mount the ramtron */ - ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /ramtron - erase ramtron to reformat"); - - started = true; - warnx("mounted ramtron at /ramtron"); - exit(0); -} - -//extern int at24c_nuke(void); - -static void -ramtron_erase(void) -{ - if (!attached) - ramtron_attach(); - -// if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -ramtron_ioctl(unsigned operation) -{ - int fd; - - fd = open("/ramtron/.", 0); - - if (fd < 0) - err(1, "open /ramtron"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -ramtron_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -ramtron_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -//extern void at24c_test(void); - -static void -ramtron_test(void) -{ -// at24c_test(); - exit(0); -} - -#endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 68a080c77c..acf28c35b2 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,8 +24,12 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ - test_rc.c + test_rc.c \ + test_conv.cpp \ + test_mount.c \ + test_mtd.c diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/systemcmds/tests/test_conv.cpp similarity index 66% rename from src/lib/mathlib/math/EulerAngles.hpp rename to src/systemcmds/tests/test_conv.cpp index 399eecfa7d..50dece816b 100644 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -32,43 +32,45 @@ ****************************************************************************/ /** - * @file Vector.h + * @file test_conv.cpp + * Tests conversions used across the system. * - * math vector */ -#pragma once +#include -#include "Vector.hpp" +#include -namespace math +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +#include +#include +#include + +int test_conv(int argc, char *argv[]) { + warnx("Testing system conversions"); -class Quaternion; -class Dcm; + for (int i = -10000; i <= 10000; i+=1) { + float f = i/10000.0f; + float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); + if (fabsf(f - fres) > 0.0001f) { + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + return 1; + } + } -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math + warnx("All conversions clean"); + return 0; +} diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1a..96be1e8df5 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -35,9 +35,12 @@ * @file test_file.c * * File write test. + * + * @author Lorenz Meier */ #include +#include #include #include #include @@ -51,6 +54,40 @@ #include "tests.h" +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + int test_file(int argc, char *argv[]) { @@ -86,7 +123,6 @@ test_file(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); @@ -94,28 +130,25 @@ test_file(int argc, char *argv[]) start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { - //perf_begin(wperf); int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) - errx(1, "memory is unaligned, align shift: %d", a); + warnx("memory is unaligned, align shift: %d", a); + return 1; } fsync(fd); - //perf_end(wperf); + + if (!check_user_abort(fd)) + return OK; } end = hrt_absolute_time(); - //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - //perf_print_counter(wperf); - //perf_free(wperf); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -124,7 +157,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - errx(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -139,9 +173,13 @@ test_file(int argc, char *argv[]) } if (!compare_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } + if (!check_user_abort(fd)) + return OK; + } /* @@ -152,16 +190,20 @@ test_file(int argc, char *argv[]) int ret = unlink("/fs/microsd/testfile"); fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned writes - please wait.."); + warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); if (wret != chunk_sizes[c]) { - err(1, "WRITE ERROR!"); + warnx("WRITE ERROR!"); + return 1; } + if (!check_user_abort(fd)) + return OK; + } fsync(fd); @@ -178,7 +220,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -190,10 +233,14 @@ test_file(int argc, char *argv[]) align_read_ok = false; break; } + + if (!check_user_abort(fd)) + return OK; } if (!align_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -215,7 +262,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf + a, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } for (int j = 0; j < chunk_sizes[c]; j++) { @@ -228,10 +276,14 @@ test_file(int argc, char *argv[]) if (unalign_read_err_count > 10) break; } + + if (!check_user_abort(fd)) + return OK; } if (!unalign_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -239,9 +291,10 @@ test_file(int argc, char *argv[]) ret = unlink("/fs/microsd/testfile"); close(fd); - if (ret) - err(1, "UNLINKING FILE FAILED"); - + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } } } @@ -261,75 +314,9 @@ test_file(int argc, char *argv[]) } else { /* failed opening dir */ - err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); - } - - return 0; -} -#if 0 -int -test_file(int argc, char *argv[]) -{ - const iterations = 1024; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); return 1; } - uint8_t buf[512]; - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, buf, sizeof(buf)); - perf_end(wperf); - } - end = hrt_absolute_time(); - - close(fd); - - unlink("/fs/microsd/testfile"); - - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - perf_print_counter(wperf); - perf_free(wperf); - - warnx("running unlink test"); - - /* ensure that common commands do not run against file count limits */ - for (unsigned i = 0; i < 64; i++) { - - warnx("unlink iteration #%u", i); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file before unlink()"); - int ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file before unlink()"); - close(fd); - - ret = unlink("/fs/microsd/testfile"); - if (ret != OK) - errx(1, "failed unlinking test file"); - - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file after unlink()"); - ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file after unlink()"); - close(fd); - } - return 0; } -#endif diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp new file mode 100644 index 0000000000..693a208ba7 --- /dev/null +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file test_mathlib.cpp + * + * Mathlib test + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + +using namespace math; + +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; +} + +int test_mathlib(int argc, char *argv[]) +{ + warnx("testing mathlib"); + + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector<2>()", Vector<2> v3); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f)); + TEST_OP("Vector<2> = Vector<2>", v = v1); + TEST_OP("Vector<2> + Vector<2>", v + v1); + TEST_OP("Vector<2> - Vector<2>", v - v1); + TEST_OP("Vector<2> += Vector<2>", v += v1); + TEST_OP("Vector<2> -= Vector<2>", v -= v1); + TEST_OP("Vector<2> * Vector<2>", v * v1); + TEST_OP("Vector<2> %% Vector<2>", v1 % v2); + } + + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector<3>()", Vector<3> v3); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector<3> = Vector<3>", v = v1); + TEST_OP("Vector<3> + Vector<3>", v + v1); + TEST_OP("Vector<3> - Vector<3>", v - v1); + TEST_OP("Vector<3> += Vector<3>", v += v1); + TEST_OP("Vector<3> -= Vector<3>", v -= v1); + TEST_OP("Vector<3> * float", v1 * 2.0f); + TEST_OP("Vector<3> / float", v1 / 2.0f); + TEST_OP("Vector<3> *= float", v1 *= 2.0f); + TEST_OP("Vector<3> /= float", v1 /= 2.0f); + TEST_OP("Vector<3> * Vector<3>", v * v1); + TEST_OP("Vector<3> %% Vector<3>", v1 % v2); + TEST_OP("Vector<3> length", v1.length()); + TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); + } + + { + Vector<4> v; + Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); + Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Vector<4> v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v * v1); + } + + { + Vector<10> v1; + v1.zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Vector<10> v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + } + + { + Matrix<3, 3> m1; + m1.identity(); + Matrix<3, 3> m2; + m2.identity(); + Vector<3> v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1); + TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); + TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); + } + + { + Matrix<10, 10> m1; + m1.identity(); + Matrix<10, 10> m2; + m2.identity(); + Vector<10> v1; + v1.zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + return 0; +} diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 4da86042d2..2a47551ee6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -48,9 +48,13 @@ #include #include #include +#include #include #include +#include +#include +#include #include "tests.h" @@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); +const unsigned output_max = 8; +static float actuator_controls[output_max]; +static bool should_arm = false; +uint16_t r_page_servo_disarmed[output_max]; +uint16_t r_page_servo_control_min[output_max]; +uint16_t r_page_servo_control_max[output_max]; +uint16_t r_page_servos[output_max]; +uint16_t servo_predicted[output_max]; + +/* + * PWM limit structure + */ +pwm_limit_t pwm_limit; + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[]) if (mixer_group.count() != 8) return 1; + /* execute the mixer */ + + float outputs[output_max]; + unsigned mixed; + const int jmax = 5; + + pwm_limit_init(&pwm_limit); + should_arm = true; + + /* run through arming phase */ + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + } + + warnx("ARMING TEST: STARTING RAMP"); + unsigned sleep_quantum_us = 10000; + + hrt_abstime starttime = hrt_absolute_time(); + unsigned sleepcount = 0; + + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (hrt_elapsed_time(&starttime) < INIT_TIME_US && + r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && + r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { + warnx("ramp servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: NORMAL OPERATION"); + + for (int j = -jmax; j <= jmax; j++) { + + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = j/10.0f + 0.1f * i; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + } + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + warnx("mixed %d outputs (max %d)", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + } + } + + warnx("ARMING TEST: DISARMING"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = false; + + while (hrt_elapsed_time(&starttime) < 600000) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: REARMING: STARTING RAMP"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = true; + + while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* predict value */ + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + + /* check ramp */ + + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && + (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || + r_page_servos[i] > servo_predicted[i])) { + warnx("ramp servo value mismatch"); + return 1; + } + + /* check post ramp phase */ + if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && + fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + /* load multirotor at once test */ mixer_group.reset(); @@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[]) unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + if (mixer_group.count() != 5) { + warnx("FAIL: Quad W mixer load failed"); return 1; + } + + warnx("SUCCESS: No errors in mixer test"); } static int @@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle, if (control_group != 0) return -1; - control = 0.0f; + if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) + return -1; + + control = actuator_controls[control_index]; return 0; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c new file mode 100644 index 0000000000..44e34d9ef3 --- /dev/null +++ b/src/systemcmds/tests/test_mount.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * 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 test_mount.c + * + * Device mount / unmount stress test + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +const int fsync_tries = 1; +const int abort_tries = 10; + +int +test_mount(int argc, char *argv[]) +{ + const unsigned iterations = 2000; + const unsigned alignments = 10; + + const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + + if (stat(cmd_filename, &buffer) == OK) { + (void)unlink(cmd_filename); + } + + return 1; + } + + /* read current test status from file, write test instructions for next round */ + + /* initial values */ + int it_left_fsync = fsync_tries; + int it_left_abort = abort_tries; + + int cmd_fd; + if (stat(cmd_filename, &buffer) == OK) { + + /* command file exists, read off state */ + cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK); + char buf[64]; + int ret = read(cmd_fd, buf, sizeof(buf)); + + if (ret > 0) { + int count = 0; + ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count); + } else { + buf[0] = '\0'; + } + + if (it_left_fsync > fsync_tries) + it_left_fsync = fsync_tries; + + if (it_left_abort > abort_tries) + it_left_abort = abort_tries; + + warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, + fsync_tries, abort_tries, buf); + + int it_left_fsync_prev = it_left_fsync; + + /* now write again what to do next */ + if (it_left_fsync > 0) + it_left_fsync--; + + if (it_left_fsync == 0 && it_left_abort > 0) { + + it_left_abort--; + + /* announce mode switch */ + if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { + warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); + fsync(stdout); + fsync(stderr); + usleep(20000); + } + + } + + if (it_left_abort == 0) { + (void)unlink(cmd_filename); + return 0; + } + + } else { + + /* this must be the first iteration, do something */ + cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT); + + warnx("First iteration of file test\n"); + } + + char buf[64]; + int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + lseek(cmd_fd, 0, SEEK_SET); + write(cmd_fd, buf, strlen(buf) + 1); + fsync(cmd_fd); + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); + fsync(stdout); + fsync(stderr); + usleep(50000); + + for (unsigned a = 0; a < alignments; a++) { + + printf("."); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + warnx("memory is unaligned, align shift: %d", a); + + return 1; + + } + + if (it_left_fsync > 0) { + fsync(fd); + } else { + printf("#"); + fsync(stdout); + fsync(stderr); + } + } + + if (it_left_fsync > 0) { + printf("#"); + } + + printf("."); + fsync(stdout); + fsync(stderr); + usleep(200000); + + end = hrt_absolute_time(); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; + } + + } + + int ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } + + } + } + + fsync(stdout); + fsync(stderr); + usleep(20000); + + + + /* we always reboot for the next test if we get here */ + warnx("Iteration done, rebooting.."); + fsync(stdout); + fsync(stderr); + usleep(50000); + systemreset(false); + + /* never going to get here */ + return 0; +} diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c new file mode 100644 index 0000000000..bac9efbdb3 --- /dev/null +++ b/src/systemcmds/tests/test_mtd.c @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * 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 test_mtd.c + * + * Param storage / file test. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +#define PARAM_FILE_NAME "/fs/mtd_params" + +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + +void print_fail() +{ + printf("<[T]: MTD: FAIL>\n"); +} + +void print_success() +{ + printf("<[T]: MTD: OK>\n"); +} + +int +test_mtd(int argc, char *argv[]) +{ + unsigned iterations= 0; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat(PARAM_FILE_NAME, &buffer)) { + warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); + print_fail(); + return 1; + } + + // XXX get real storage space here + unsigned file_size = 4096; + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {256, 512, 4096}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open(PARAM_FILE_NAME, O_RDONLY); + int rret = read(fd, read_buf, chunk_sizes[c]); + close(fd); + + fd = open(PARAM_FILE_NAME, O_WRONLY); + + printf("printing 2 percent of the first chunk:\n"); + for (int i = 0; i < sizeof(read_buf) / 50; i++) { + printf("%02X", read_buf[i]); + } + printf("\n"); + + iterations = file_size / chunk_sizes[c]; + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != (int)chunk_sizes[c]) { + warn("WRITE ERROR!"); + print_fail(); + return 1; + } + + fsync(fd); + + if (!check_user_abort(fd)) + return OK; + + } + end = hrt_absolute_time(); + + close(fd); + fd = open(PARAM_FILE_NAME, O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + print_fail(); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d", j); + print_fail(); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + print_fail(); + return 1; + } + + if (!check_user_abort(fd)) + return OK; + + } + + + close(fd); + + } + + /* fill the file with 0xFF to make it look new again */ + char ffbuf[64]; + memset(ffbuf, 0xFF, sizeof(ffbuf)); + int fd = open(PARAM_FILE_NAME, O_WRONLY); + for (int i = 0; i < file_size / sizeof(ffbuf); i++) { + int ret = write(fd, ffbuf, sizeof(ffbuf)); + + if (ret != sizeof(ffbuf)) { + warnx("ERROR! Could not fill file with 0xFF"); + close(fd); + print_fail(); + return 1; + } + } + + (void)close(fd); + print_success(); + + return 0; +} diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 72619fc8ba..6a602ecfc9 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file test_ppm_loopback.c - * Tests the PWM outputs and PPM input + * @file test_rc.c + * Tests RC input. * */ diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index a57d04be37..ac64ad33d3 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -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 @@ -109,6 +109,10 @@ extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); +extern int test_conv(int argc, char *argv[]); +extern int test_mount(int argc, char *argv[]); +extern int test_mtd(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 1088a44076..73827b7cf0 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -106,6 +106,10 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mtd", test_mtd, 0}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 1e3d2acbf66b1101a9b17f97d2b786ffaa0e423a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 19:30:23 +0100 Subject: [PATCH 06/99] Not building yet, things are coming together slowly on mavlink app --- src/modules/mavlink/mavlink_main.cpp | 18 +- src/modules/mavlink/mavlink_main.h | 50 +- src/modules/mavlink/mavlink_orb_listener.cpp | 545 ++++++++++--------- src/modules/mavlink/mavlink_orb_listener.h | 94 +++- src/modules/mavlink/mavlink_receiver.cpp | 26 +- src/modules/mavlink/mavlink_receiver.h | 4 +- 6 files changed, 421 insertions(+), 316 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1c7986cbb8..cd37c5437c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -196,6 +195,11 @@ Mavlink::~Mavlink() } } +void Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ @@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + _mode = MODE_ONBOARD; break; default: @@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - MavlinkReceiver rcv(this); - receive_thread = rcv.receive_start(uart); + // MavlinkReceiver rcv(this); + receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ - MavlinkOrbListener listener(this); - uorb_receive_thread = listener.uorb_receive_start(); + //MavlinkOrbListener listener(this); + uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 244af04a67..3b6714559f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -54,9 +54,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -72,7 +70,8 @@ #include #include #include - +#include +#include #include @@ -139,6 +138,7 @@ struct mavlink_wpm_storage { class Mavlink { + public: /** * Constructor @@ -170,6 +170,31 @@ public: static int get_uart_fd(unsigned index); + int get_uart_fd() { return _mavlink_fd; } + + enum MAVLINK_MODE { + MODE_TX_HEARTBEAT_ONLY=0, + MODE_OFFBOARD, + MODE_ONBOARD, + MODE_HIL + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode(); + + bool hil_enabled() { return _mavlink_hil_enabled; }; + + /** + * Handle waypoint related messages. + */ + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + struct mavlink_subscriptions { int sensor_sub; int att_sub; @@ -196,10 +221,15 @@ public: int airspeed_sub; int navigation_capabilities_sub; int control_mode_sub; + int rc_sub; + int status_sub; }; struct mavlink_subscriptions subs; + struct mavlink_subscriptions* get_subs() { return &subs; } + mavlink_channel_t get_chan() { return chan; } + /** Global position */ struct vehicle_global_position_s global_pos; /** Local position */ @@ -243,6 +273,7 @@ private: uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER bool thread_running; bool thread_should_exit; + MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; mavlink_channel_t chan; @@ -269,12 +300,6 @@ private: */ unsigned int mavlink_param_queue_index; - /* interface mode */ - enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD - } mavlink_link_mode; - struct mavlink_logbuffer lb; bool mavlink_link_termination_allowed; @@ -293,12 +318,6 @@ private: */ int set_hil_on_off(bool hil_enabled); - - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - /** * Send all parameters at once. * @@ -352,7 +371,6 @@ private: void mavlink_update_system(); - void mavlink_wpm_message_handler(const mavlink_message_t *msg); void mavlink_waypoint_eventloop(uint64_t now); void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 44bf77bb0d..fa1a6887ef 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -62,11 +62,7 @@ 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); @@ -86,41 +82,58 @@ cm_uint16_from_m_float(float m) MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - _task_should_exit(false), + thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), + _listeners(nullptr), _n_listeners(0) { - 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}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, -}; + add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0); + add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0); + add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0); + add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0); + add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0); + add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0); + add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0); + add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3); + add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0); + add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0); + add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0); + add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0); + add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0); + add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0); + add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0); + add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0); + add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0); - _n_listeners = sizeof(listeners) / sizeof(listeners[0]); +} +void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg) +{ + struct listener *nl = new listener; + + nl->callback = callback; + nl->subp = subp; + nl->arg = arg; + nl->next = nullptr; + + // Register it + struct listener *next = _listeners; + while (next->next != nullptr) { + next = next->next; + } + + // Attach + next->next = nl; + _n_listeners++; } void @@ -129,43 +142,37 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - last_sensor_timestamp = raw.timestamp; - - /* mark individual fields as changed */ + /* mark individual fields as _mavlink->get_chan()ged */ 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 (accel_counter != raw.accelerometer_counter) { + // /* mark first three dimensions as _mavlink->get_chan()ged */ + // 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 (gyro_counter != raw.gyro_counter) { + // /* mark second group dimensions as _mavlink->get_chan()ged */ + // 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 (mag_counter != raw.magnetometer_counter) { + // /* mark third group dimensions as _mavlink->get_chan()ged */ + // 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 (baro_counter != raw.baro_counter) { + // /* mark last group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + // baro_counter = raw.baro_counter; + // } - if (gcs_link) - mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp, + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->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], @@ -175,51 +182,51 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l) raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); - sensors_raw_counter++; + l->listener->sensors_raw_counter++; } void MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) { /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { /* send sensor values */ - mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); + mavlink_msg_attitude_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, + l->listener->att.roll, + l->listener->att.pitch, + l->listener->att.yaw, + l->listener->att.rollspeed, + l->listener->att.pitchspeed, + l->listener->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->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); + if (t >= l->listener->last_sent_vfr + 100000) { + l->listener->last_sent_vfr = t; + float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); + uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; + float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; + mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); } /* send quaternion values if it exists */ - if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); + if(l->listener->att.q_valid) { + mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, + l->listener->att.q[0], + l->listener->att.q[1], + l->listener->att.q[2], + l->listener->att.q[3], + l->listener->att.rollspeed, + l->listener->att.pitchspeed, + l->listener->att.yawspeed); } } - attitude_counter++; + l->listener->attitude_counter++; } void @@ -228,13 +235,13 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_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->get_mavlink_chan(), + mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), gps.timestamp_position, gps.fix_type, gps.lat, @@ -247,8 +254,8 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) gps.satellites_visible); /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(), + if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { + mavlink_msg_gps_status_send(l->mavlink->get_chan(), gps.satellites_visible, gps.satellite_prn, gps.satellite_used, @@ -257,30 +264,30 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) gps.satellite_snr); } - gps_counter++; + l->listener->gps_counter++; } void MavlinkOrbListener::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); + /* immediately communicate state _mavlink->get_chan()ges back to user */ + orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status); + orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); /* 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); + if (l->listener->v_status.hil_state == HIL_STATE_ON) + l->mavlink->set_hil_on_off(true); + else if (l->listener->v_status.hil_state == HIL_STATE_OFF) + l->mavlink->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); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, + mavlink_msg_heartbeat_send(l->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -289,37 +296,37 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) } void -MavlinkOrbListener::l_rc_channels(const struct listener *l) +MavlinkOrbListener::l_rc__mavlink->get_chan()nels(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 + /* copy rc _mavlink->get_chan()nels into local buffer */ + orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc); + // XXX Add RC _mavlink->get_chan()nels scaled message here } void MavlinkOrbListener::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); + /* copy rc _mavlink->get_chan()nels into local buffer */ + orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(), + l->listener->rc_raw.timestamp / 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); + (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, + l->listener->rc_raw.rssi); } } } @@ -328,48 +335,48 @@ void MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos); - mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), - 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); + mavlink_msg_global_position_int_send(l->mavlink->get_chan(), + l->listener->global_pos.timestamp / 1000, + l->listener->global_pos.lat * 1e7, + l->listener->global_pos.lon * 1e7, + l->listener->global_pos.alt * 1000.0f, + (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, + l->listener->global_pos.vel_n * 100.0f, + l->listener->global_pos.vel_e * 100.0f, + l->listener->global_pos.vel_d * 100.0f, + _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos); - if (gcs_link) - mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(), - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), + l->listener->local_pos.timestamp / 1000, + l->listener->local_pos.x, + l->listener->local_pos.y, + l->listener->local_pos.z, + l->listener->local_pos.vx, + l->listener->local_pos.vy, + l->listener->local_pos.vz); } void MavlinkOrbListener::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); + orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); if (!triplet.current.valid) return; - if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), @@ -383,10 +390,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); - if (gcs_link) - mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -400,10 +407,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp); - if (gcs_link) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), att_sp.timestamp / 1000, att_sp.roll_body, att_sp.pitch_body, @@ -417,10 +424,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); - if (gcs_link) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), rates_sp.timestamp / 1000, rates_sp.roll, rates_sp.pitch, @@ -443,8 +450,8 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) /* 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->get_mavlink_chan(), last_sensor_timestamp / 1000, + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { + mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, l->arg /* port number - needs GCS support */, /* QGC has port number support already */ act_outputs.output[0], @@ -457,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 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)) { + if (l->listener->mavlink_hil_enabled && l->listener->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; @@ -470,7 +477,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -484,7 +491,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -498,7 +505,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -512,7 +519,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 500.0f, (act_outputs.output[1] - 1500.0f) / 500.0f, @@ -532,7 +539,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) void MavlinkOrbListener::l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); } void @@ -541,10 +548,10 @@ MavlinkOrbListener::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); + orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); - if (gcs_link) - mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_manual_control_send(l->mavlink->get_chan(), mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -556,26 +563,26 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) void MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); - if (gcs_link) { + 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(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl0 ", - actuators_0.control[0]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[0]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl1 ", - actuators_0.control[1]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[1]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl2 ", - actuators_0.control[2]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[2]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl3 ", - actuators_0.control[3]); + l->listener->actuators_0.control[3]); } } @@ -584,13 +591,13 @@ MavlinkOrbListener::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); + 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(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, debug.key, debug.value); } @@ -600,52 +607,52 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) { struct optical_flow_s flow; - orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); + orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); - mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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 MavlinkOrbListener::l_home(const struct listener *l) { - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home); - mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); + mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void MavlinkOrbListener::l_airspeed(const struct listener *l) { - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); + orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); } void MavlinkOrbListener::l_nav_cap(const struct listener *l) { - orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), hrt_absolute_time() / 1000, "turn dist", - nav_cap.turn_distance); + l->listener->nav_cap.turn_distance); } void MavlinkOrbListener::l_control_mode(const struct listener *l) { - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); + orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode); /* 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); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, + mavlink_msg_heartbeat_send(l->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -657,7 +664,9 @@ void * MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); + char buf[32]; + sprintf(buf, "mavlink rcv%d", Mavlink::instance_count()); + prctl(PR_SET_NAME, buf, getpid()); /* * set up poll to block for new data, @@ -670,15 +679,21 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) * * Might want to invoke each listener once to set initial state. */ - struct pollfd fds[_n_listeners]; + struct pollfd fds[_max_listeners]; - for (unsigned i = 0; i < _n_listeners; i++) { - fds[i].fd = *listeners[i].subp; + struct listener* next = _listeners; + unsigned i = 0; + + while (next != nullptr) { + + fds[i].fd = *next->subp; fds[i].events = POLLIN; + next = next->next; + i++; + } /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); - } while (!thread_should_exit) { @@ -689,13 +704,19 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) /* silent */ } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + //mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); } else { - for (unsigned i = 0; i < _n_listeners; i++) { + unsigned i = 0; + struct listener* cb = _listeners; + while (cb != nullptr) { + if (fds[i].revents & POLLIN) - listeners[i].callback(&listeners[i]); + cb->callback(cb); + + cb = cb->next; + i++; } } } @@ -703,107 +724,113 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) return NULL; } -pthread_t -MavlinkOrbListener::uorb_receive_start(void) +void * MavlinkOrbListener::uorb_start_helper(void *context) { + return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL); +} + +pthread_t +MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) +{ + MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink); + /* --- SENSORS RAW VALUE --- */ - mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */ /* --- ATTITUDE VALUE --- */ - mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ /* --- CONTROL MODE --- */ - mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(mavlink->get_subs()->control_mode_sub, 300); /* max 3.33 Hz updates */ /* --- RC CHANNELS VALUE --- */ - rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ + mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(mavlink->get_subs()->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); + mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink->get_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)); + mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + mavlink->get_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 */ + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink->get_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 */ + mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink->get_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; + mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */ /* start the listener loop */ pthread_attr_t uorb_attr; @@ -813,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(void) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv); pthread_attr_destroy(&uorb_attr); return thread; diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 29e081b360..3988103bcf 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -38,8 +38,40 @@ * @author Lorenz Meier */ +#include + #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + class Mavlink; class MavlinkOrbListener @@ -67,18 +99,32 @@ public: */ void status(); - pthread_t uorb_receive_start(void); + static pthread_t uorb_receive_start(Mavlink *mavlink); void * uorb_receive_thread(void *arg); + struct listener { + void (*callback)(const struct listener *l); + int *subp; + uintptr_t arg; + struct listener* next; + Mavlink *mavlink; + MavlinkOrbListener* listener; + }; + + void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg); + static void * uorb_start_helper(void *context); + private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool thread_should_exit; /**< if true, sensor task should exit */ perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; + struct listener *_listeners; unsigned _n_listeners; + static const unsigned _max_listeners = 32; /** * Shim for calling task_main from task_create. @@ -90,28 +136,28 @@ private: */ void task_main() __attribute__((noreturn)); - void l_sensor_combined(const struct listener *l); - void l_vehicle_attitude(const struct listener *l); - void l_vehicle_gps_position(const struct listener *l); - void l_vehicle_status(const struct listener *l); - void l_rc_channels(const struct listener *l); - void l_input_rc(const struct listener *l); - void l_global_position(const struct listener *l); - void l_local_position(const struct listener *l); - void l_global_position_setpoint(const struct listener *l); - void l_local_position_setpoint(const struct listener *l); - void l_attitude_setpoint(const struct listener *l); - void l_actuator_outputs(const struct listener *l); - void l_actuator_armed(const struct listener *l); - void l_manual_control_setpoint(const struct listener *l); - void l_vehicle_attitude_controls(const struct listener *l); - void l_debug_key_value(const struct listener *l); - void l_optical_flow(const struct listener *l); - void l_vehicle_rates_setpoint(const struct listener *l); - void l_home(const struct listener *l); - void l_airspeed(const struct listener *l); - void l_nav_cap(const struct listener *l); - void l_control_mode(const struct listener *l); + 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 void l_control_mode(const struct listener *l); struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f763c3c68..982d6c1d82 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -83,7 +83,8 @@ __BEGIN_DECLS __END_DECLS -void MavlinkReceiver::MavlinkReceiver() : +MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : + _mavlink(parent), pub_hil_global_pos(-1), pub_hil_local_pos(-1), pub_hil_attitude(-1), @@ -790,7 +791,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { - int uart_fd = *((int *)arg); + int uart_fd = _mavlink->get_uart_fd(); const int timeout = 1000; uint8_t buf[32]; @@ -822,10 +823,10 @@ MavlinkReceiver::receive_thread(void *arg) 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); } } } @@ -839,15 +840,22 @@ void MavlinkReceiver::print_status() } -pthread_t -MavlinkReceiver::receive_start(int uart) +void * MavlinkReceiver::start_helper(void *context) { + return ((MavlinkReceiver *)context)->receive_thread(NULL); +} + +pthread_t +MavlinkReceiver::receive_start(Mavlink *mavlink) +{ + MavlinkReceiver *rcv = new MavlinkReceiver(mavlink); + 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(mavlink->get_uart_fd(), F_GETFL, 0); + fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -857,7 +865,7 @@ MavlinkReceiver::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, rcv); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ea57714d2d..483d91e725 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -96,7 +96,9 @@ public: */ void print_status(); - pthread_t receive_start(int uart); + static pthread_t receive_start(Mavlink* mavlink); + + static void * start_helper(void *context); private: From 63b18399c26acb1e3cf771376c3376b4d00a407a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 21:05:00 +0100 Subject: [PATCH 07/99] Butchered MAVLink C++ app to compile and link - there is no hope it will work out of the box 8) --- src/modules/mavlink/mavlink_bridge_header.h | 6 +- src/modules/mavlink/mavlink_main.cpp | 124 ++++++++++--------- src/modules/mavlink/mavlink_main.h | 42 +++---- src/modules/mavlink/mavlink_orb_listener.cpp | 24 ++-- src/modules/mavlink/mavlink_orb_listener.h | 1 + src/modules/mavlink/mavlink_receiver.cpp | 5 +- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/mavlink/module.mk | 3 +- 8 files changed, 106 insertions(+), 101 deletions(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 1fae3ee9d0..374d1511c1 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -42,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 */ @@ -72,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 +__END_DECLS + #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd37c5437c..6c04d2aba0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -97,6 +97,8 @@ #endif static const int ERROR = -1; +static Mavlink* _head = nullptr; + /** * mavlink app start / stop handling function * @@ -203,10 +205,10 @@ void Mavlink::set_mode(enum MAVLINK_MODE mode) int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ - Mavlink* inst = _head; + Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_head != nullptr) { - inst = inst->_head; + while (inst->_next != nullptr) { + inst = inst->_next; inst_index++; } @@ -216,22 +218,22 @@ int Mavlink::instance_count() Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) { Mavlink* inst = new Mavlink(port, baud_rate); - Mavlink* parent = _head; - while (parent->_head != nullptr) - parent = parent->_head; + Mavlink* parent = ::_head; + while (parent->_next != nullptr) + parent = parent->_next; /* now parent points to a null pointer, fill it */ - parent->_head = inst; + parent->_next = inst; return inst; } Mavlink* Mavlink::get_instance(unsigned instance) { - Mavlink* inst = _head; + Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_head != nullptr && inst_index < instance) { - inst = inst->_head; + while (inst->_next != nullptr && inst_index < instance) { + inst = inst->_next; inst_index++; } @@ -428,9 +430,9 @@ Mavlink::set_hil_on_off(bool hil_enabled) int ret = OK; /* Enable HIL */ - if (hil_enabled && !mavlink_hil_enabled) { + if (hil_enabled && !_mavlink_hil_enabled) { - mavlink_hil_enabled = true; + _mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; @@ -456,8 +458,8 @@ Mavlink::set_hil_on_off(bool hil_enabled) set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } - if (!hil_enabled && mavlink_hil_enabled) { - mavlink_hil_enabled = false; + if (!hil_enabled && _mavlink_hil_enabled) { + _mavlink_hil_enabled = false; orb_set_interval(subs.spa_sub, 200); } else { @@ -1426,12 +1428,6 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -void -Mavlink::task_main_trampoline(int argc, char *argv[]) -{ - mavlink::g_mavlink->task_main(argc, argv); -} - int Mavlink::task_main(int argc, char *argv[]) { @@ -1481,43 +1477,43 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 10); + // 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; + // /* 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); + // 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); + // if (baudrate < 9600 || baudrate > 921600) + // errx(1, "invalid baud rate '%s'", optarg); - break; + // break; - case 'd': - device_name = optarg; - break; + // case 'd': + // device_name = optarg; + // break; - case 'e': - mavlink_link_termination_allowed = true; - break; + // case 'e': + // mavlink_link_termination_allowed = true; + // break; - case 'o': - _mode = MODE_ONBOARD; - break; + // case 'o': + // _mode = MODE_ONBOARD; + // break; - default: - usage(); - break; - } - } + // default: + // usage(); + // break; + // } + // } struct termios uart_config_original; @@ -1699,15 +1695,15 @@ Mavlink::task_main(int argc, char *argv[]) /* 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); + // /* 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); - } - } + // if (lb_ret == OK) { + // mavlink_missionlib_send_gcs_string(msg.text); + // } + // } /* sleep 15 ms */ usleep(15000); @@ -1742,27 +1738,32 @@ Mavlink::task_main(int argc, char *argv[]) _exit(0); } -int -Mavlink::start() +int Mavlink::start_helper(int argc, char *argv[]) +{ + // This is beyond evil.. and needs a lock to be safe + return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv); +} + +int +Mavlink::start(Mavlink* mavlink) { - ASSERT(_mavlink_task == -1); /* start the task */ char buf[32]; sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - _mavlink_task = task_spawn_cmd(buf, + mavlink->_mavlink_task = task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, - (main_t)&Mavlink::task_main_trampoline, - nullptr); + (main_t)&Mavlink::start_helper, + NULL); // while (!this->is_running()) { // usleep(200); // } - if (_mavlink_task < 0) { + if (mavlink->_mavlink_task < 0) { warn("task start failed"); return -errno; } @@ -1794,6 +1795,9 @@ int mavlink_main(int argc, char *argv[]) if (mavlink::g_mavlink == nullptr) mavlink::g_mavlink = instance; + // Instantiate thread + Mavlink::start(instance); + // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); // } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 3b6714559f..e50b0f0c0f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -155,7 +155,7 @@ public: * * @return OK on success. */ - int start(); + static int start(Mavlink* mavlink); /** * Display the mavlink status. @@ -170,7 +170,7 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return _mavlink_fd; } + int get_uart_fd() { return uart; } enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, @@ -180,7 +180,7 @@ public: }; void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode(); + enum MAVLINK_MODE get_mode() { return _mode; } bool hil_enabled() { return _mavlink_hil_enabled; }; @@ -189,11 +189,24 @@ public: */ 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(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_on_off(bool hil_enabled); struct mavlink_subscriptions { int sensor_sub; @@ -217,12 +230,13 @@ public: int input_rc_sub; int optical_flow; int rates_setpoint_sub; - int home_sub; + int get_sub; int airspeed_sub; int navigation_capabilities_sub; int control_mode_sub; int rc_sub; int status_sub; + int home_sub; }; struct mavlink_subscriptions subs; @@ -252,6 +266,7 @@ protected: */ struct file_operations fops; int _mavlink_fd; + Mavlink* _next; private: @@ -264,7 +279,6 @@ private: /* states */ bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ - static Mavlink* _head; int _params_sub; @@ -289,7 +303,6 @@ private: mavlink_wpm_storage *wpm; bool verbose; - bool mavlink_hil_enabled; int uart; int baudrate; bool gcs_link; @@ -308,16 +321,6 @@ private: */ void parameters_update(); - /** - * 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_on_off(bool hil_enabled); - /** * Send all parameters at once. * @@ -390,8 +393,6 @@ private: int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - /** * Callback for param interface. */ @@ -399,11 +400,6 @@ private: static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - /** * Main mavlink task. */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index fa1a6887ef..a6bcd4c0a2 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -296,11 +296,11 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) } void -MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l) +MavlinkOrbListener::l_rc_channels(const struct listener *l) { - /* copy rc _mavlink->get_chan()nels into local buffer */ - orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc); - // XXX Add RC _mavlink->get_chan()nels scaled message here + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); + // XXX Add RC channels scaled message here } void @@ -315,7 +315,7 @@ MavlinkOrbListener::l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(), + mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), l->listener->rc_raw.timestamp / 1000, i, (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, @@ -335,7 +335,7 @@ void MavlinkOrbListener::l_global_position(const struct listener *l) { /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos); + orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); mavlink_msg_global_position_int_send(l->mavlink->get_chan(), l->listener->global_pos.timestamp / 1000, @@ -353,7 +353,7 @@ void MavlinkOrbListener::l_local_position(const struct listener *l) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos); + orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), @@ -407,7 +407,7 @@ MavlinkOrbListener::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->get_subs()->spa_sub, &att_sp); + orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), @@ -464,13 +464,13 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ - if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + if (l->mavlink->hil_enabled() && l->listener->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); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -616,9 +616,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) void MavlinkOrbListener::l_home(const struct listener *l) { - orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home); + orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); - mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); + mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); } void diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 3988103bcf..c9f35a1fbd 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -170,6 +170,7 @@ private: struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct airspeed_s airspeed; + struct home_position_s home; int status_sub; int rc_sub; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 982d6c1d82..0f1d5293ef 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0) + alt0(0), + thread_should_exit(false) { } @@ -818,7 +819,7 @@ MavlinkReceiver::receive_thread(void *arg) /* 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_chan(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 483d91e725..be32ce0f74 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -102,7 +102,7 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool thread_should_exit; /**< if true, sensor task should exit */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 2a005565e0..76798eb124 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp \ - waypoints.cpp + mavlink_orb_listener.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From c73fbca0ebef6a0333b364bfe90891b580eccace Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:13 +0100 Subject: [PATCH 08/99] Move instance to new task context --- src/modules/mavlink/mavlink_main.cpp | 33 +++++++++++++--------------- src/modules/mavlink/mavlink_main.h | 6 ++--- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c04d2aba0..f70c3ce759 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -156,7 +156,7 @@ namespace mavlink Mavlink *g_mavlink; } -Mavlink::Mavlink(const char *port, unsigned baud_rate) : +Mavlink::Mavlink() : _task_should_exit(false), _mavlink_task(-1), @@ -215,9 +215,9 @@ int Mavlink::instance_count() return inst_index + 1; } -Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) +Mavlink* Mavlink::new_instance() { - Mavlink* inst = new Mavlink(port, baud_rate); + Mavlink* inst = new Mavlink(); Mavlink* parent = ::_head; while (parent->_next != nullptr) parent = parent->_next; @@ -1740,21 +1740,23 @@ Mavlink::task_main(int argc, char *argv[]) int Mavlink::start_helper(int argc, char *argv[]) { - // This is beyond evil.. and needs a lock to be safe - return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, 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(Mavlink* mavlink) +Mavlink::start() { /* start the task */ char buf[32]; sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - mavlink->_mavlink_task = task_spawn_cmd(buf, + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT, 2048, (main_t)&Mavlink::start_helper, NULL); @@ -1763,10 +1765,10 @@ Mavlink::start(Mavlink* mavlink) // usleep(200); // } - if (mavlink->_mavlink_task < 0) { - warn("task start failed"); - return -errno; - } + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } return OK; } @@ -1790,13 +1792,8 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - Mavlink *instance = Mavlink::new_instance("/dev/ttyS0", 57600); - - if (mavlink::g_mavlink == nullptr) - mavlink::g_mavlink = instance; - // Instantiate thread - Mavlink::start(instance); + Mavlink::start(); // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e50b0f0c0f..9e2475e2ba 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -143,7 +143,7 @@ public: /** * Constructor */ - Mavlink(const char *port, unsigned baud_rate); + Mavlink(); /** * Destructor, also kills the mavlinks task. @@ -155,7 +155,7 @@ public: * * @return OK on success. */ - static int start(Mavlink* mavlink); + static int start(); /** * Display the mavlink status. @@ -164,7 +164,7 @@ public: static int instance_count(); - static Mavlink* new_instance(const char *port, unsigned baud_rate); + static Mavlink* new_instance(); static Mavlink* get_instance(unsigned instance); From a5e02db6d55f246ae71bd5de356dfe17b89bf26d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:36 +0100 Subject: [PATCH 09/99] Move orb listener to new thread context --- src/modules/mavlink/mavlink_orb_listener.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index a6bcd4c0a2..ffff1838c9 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -726,13 +726,13 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) void * MavlinkOrbListener::uorb_start_helper(void *context) { - return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL); + MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context)); + return urcv->uorb_receive_thread(NULL); } pthread_t MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) { - MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink); /* --- SENSORS RAW VALUE --- */ mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -840,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink); pthread_attr_destroy(&uorb_attr); return thread; From b1e5304a3f50975a1d282bf5b7418ff276a26c45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:53 +0100 Subject: [PATCH 10/99] Move serial port listener to new thread context --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0f1d5293ef..3752bde104 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -843,13 +843,13 @@ void MavlinkReceiver::print_status() void * MavlinkReceiver::start_helper(void *context) { - return ((MavlinkReceiver *)context)->receive_thread(NULL); + MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context)); + return rcv->receive_thread(NULL); } pthread_t MavlinkReceiver::receive_start(Mavlink *mavlink) { - MavlinkReceiver *rcv = new MavlinkReceiver(mavlink); pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); @@ -866,7 +866,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink); pthread_attr_destroy(&receiveloop_attr); return thread; From 62d3369dc94458ed30b9326814296cd9a33a3cc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 11:06:53 +0100 Subject: [PATCH 11/99] Fixed init --- src/modules/mavlink/mavlink_main.cpp | 180 +++++++++++---------------- src/modules/mavlink/mavlink_main.h | 7 +- 2 files changed, 79 insertions(+), 108 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c04d2aba0..c29d9957de 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,6 +159,7 @@ namespace mavlink Mavlink::Mavlink(const char *port, unsigned baud_rate) : _task_should_exit(false), + thread_running(false), _mavlink_task(-1), _mavlink_fd(-1), _mavlink_incoming_fd(-1), @@ -207,23 +208,23 @@ int Mavlink::instance_count() /* note: a local buffer count will help if this ever is called often */ Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_next != nullptr) { + while (inst != nullptr) { inst = inst->_next; inst_index++; } - return inst_index + 1; + return inst_index; } Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) { Mavlink* inst = new Mavlink(port, baud_rate); - Mavlink* parent = ::_head; - while (parent->_next != nullptr) - parent = parent->_next; + Mavlink* next = ::_head; + while (next != nullptr) + next = next->_next; - /* now parent points to a null pointer, fill it */ - parent->_next = inst; + /* now parent has a null pointer, fill it */ + next = inst; return inst; } @@ -377,7 +378,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * 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); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); return -EINVAL; } @@ -437,15 +438,15 @@ Mavlink::set_hil_on_off(bool hil_enabled) /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; - if (baudrate < 19200) { + if (_baudrate < 19200) { /* 10 Hz */ hil_rate_interval = 100; - } else if (baudrate < 38400) { + } else if (_baudrate < 38400) { /* 10 Hz */ hil_rate_interval = 100; - } else if (baudrate < 115200) { + } else if (_baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; @@ -1438,82 +1439,48 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize logging device */ // YYY - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - - /* wakeup source(s) */ - struct pollfd fds[1]; - - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; - - while (!_task_should_exit) { - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - perf_begin(_loop_perf); - - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - } - - - + _mavlink_fd = 0;//open(MAVLINK_LOG_DEVICE, 0); + //mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* initialize mavlink text message buffering */ // mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; - baudrate = 57600; + _baudrate = 57600; - // /* work around some stupidity in task_create's argv handling */ - // argc -= 2; - // argv += 2; + /* 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); + 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); + if (_baudrate < 9600 || _baudrate > 921600) + errx(1, "invalid baud rate '%s'", optarg); - // break; + break; - // case 'd': - // device_name = optarg; - // break; + case 'd': + device_name = optarg; + break; - // case 'e': - // mavlink_link_termination_allowed = true; - // break; + case 'e': + mavlink_link_termination_allowed = true; + break; - // case 'o': - // _mode = MODE_ONBOARD; - // break; + case 'o': + _mode = MODE_ONBOARD; + break; - // default: - // usage(); - // break; - // } - // } + default: + usage(); + break; + } + } struct termios uart_config_original; @@ -1529,13 +1496,13 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* default values for arguments */ - uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + 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, &fops, 0666, NULL); + //register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* Initialize system properties */ mavlink_update_system(); @@ -1552,7 +1519,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_init(wpm); /* all subscriptions are now active, set up initial guess about rate limits */ - if (baudrate >= 230400) { + if (_baudrate >= 230400) { /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); @@ -1566,7 +1533,7 @@ Mavlink::task_main(int argc, char *argv[]) /* 10 Hz */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - } else if (baudrate >= 115200) { + } else if (_baudrate >= 115200) { /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); @@ -1579,7 +1546,7 @@ Mavlink::task_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - } else if (baudrate >= 57600) { + } else if (_baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); @@ -1616,7 +1583,35 @@ Mavlink::task_main(int argc, char *argv[]) /* arm counter to go off immediately */ unsigned lowspeed_counter = 10; - while (!thread_should_exit) { + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* parameters updated */ + if (fds[0].revents & POLLIN) { + parameters_update(); + } /* 1 Hz */ if (lowspeed_counter == 10) { @@ -1670,31 +1665,20 @@ Mavlink::task_main(int argc, char *argv[]) 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) { + 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; @@ -1705,8 +1689,7 @@ Mavlink::task_main(int argc, char *argv[]) // } // } - /* sleep 15 ms */ - usleep(15000); + perf_end(_loop_perf); } /* wait for threads to complete */ @@ -1721,17 +1704,6 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = false; - return 0; - - - - - - - - perf_end(_loop_perf); - } - warnx("exiting."); _mavlink_task = -1; @@ -1755,7 +1727,7 @@ Mavlink::start(Mavlink* mavlink) mavlink->_mavlink_task = task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 4096, (main_t)&Mavlink::start_helper, NULL); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e50b0f0c0f..34e2b59eea 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -270,7 +270,8 @@ protected: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_should_exit; /**< if true, mavlink task should exit */ + bool thread_running; int _mavlink_task; /**< task handle for sensor task */ int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ @@ -285,8 +286,6 @@ private: orb_advert_t mission_pub; struct mission_s mission; uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER - bool thread_running; - bool thread_should_exit; MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; @@ -304,7 +303,7 @@ private: bool verbose; int uart; - int baudrate; + int _baudrate; bool gcs_link; /** * If the queue index is not at 0, the queue sending From 1e6011cc87042b40cf02705deb33f29f3afdb5a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 16:59:39 +0100 Subject: [PATCH 12/99] Stop the through hoop jumping now that we can handle multiple interfaces --- ROMFS/px4fmu_common/init.d/rc.usb | 31 ------------------------------- 1 file changed, 31 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0cd8a0e042..3d8be089ee 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.." - # Exit shell to make it available to MAVLink exit From 8d2d171bb83c4f24f709c20ce8c89b6d1f529616 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 17:00:16 +0100 Subject: [PATCH 13/99] Fixes and cleanups --- src/modules/mavlink/mavlink_main.cpp | 54 +++++++++++++--------------- 1 file changed, 25 insertions(+), 29 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fa05758664..b5bf9ece0b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -144,7 +144,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + size_t desired = (size_t)(sizeof(uint8_t) * length); + int ret = write(uart, ch, desired); + + if (ret != desired) + warn("write err"); } @@ -1509,11 +1513,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ // MavlinkReceiver rcv(this); - receive_thread = MavlinkReceiver::receive_start(this); + //receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ //MavlinkOrbListener listener(this); - uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); + //uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1595,11 +1599,6 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); @@ -1722,26 +1721,6 @@ int Mavlink::start() { - /* start the task */ - char buf[32]; - sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - NULL); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - return OK; } @@ -1765,7 +1744,24 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { // Instantiate thread - Mavlink::start(); + char buf[32]; + sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); From a5045ccee663c500011b8a5a94554f4cbb263352 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 14:38:18 +0100 Subject: [PATCH 14/99] Mavlink: get rid of some warnings, initialize channel --- src/modules/mavlink/mavlink_main.cpp | 54 +++++++++----------- src/modules/mavlink/mavlink_main.h | 8 +-- src/modules/mavlink/mavlink_orb_listener.cpp | 9 ++-- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 4 files changed, 32 insertions(+), 41 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b5bf9ece0b..21ba51b21c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -36,15 +36,18 @@ * MAVLink 1.0 protocol implementation. * * @author Lorenz Meier + * @author Julian Oes */ #include #include #include #include +#include #include #include #include +#include #include #include #include @@ -66,27 +69,15 @@ #include #include #include +#include #include #include #include #include #include - #include "mavlink_bridge_header.h" -#include #include "math.h" /* isinf / isnan checks */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include "mavlink_main.h" #include "mavlink_orb_listener.h" #include "mavlink_receiver.h" @@ -144,8 +135,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - size_t desired = (size_t)(sizeof(uint8_t) * length); - int ret = write(uart, ch, desired); + warnx("uart: %d", uart); + warnx("channel: %d", channel); + + ssize_t desired = (sizeof(uint8_t) * length); + ssize_t ret = write(uart, ch, desired); if (ret != desired) warn("write err"); @@ -160,12 +154,11 @@ namespace mavlink Mavlink *g_mavlink; } -Mavlink::Mavlink() : - +Mavlink::Mavlink() : + _mavlink_fd(-1), _task_should_exit(false), thread_running(false), _mavlink_task(-1), - _mavlink_fd(-1), _mavlink_incoming_fd(-1), /* performance counters */ @@ -276,8 +269,6 @@ Mavlink::parameters_update() int Mavlink::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: @@ -430,7 +421,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } int -Mavlink::set_hil_on_off(bool hil_enabled) +Mavlink::set_hil_enabled(bool hil_enabled) { int ret = OK; @@ -1398,7 +1389,7 @@ 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(chan, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len); } @@ -1441,9 +1432,8 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize logging device */ - // YYY - - _mavlink_fd = 0;//open(MAVLINK_LOG_DEVICE, 0); + // TODO + _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); //mavlink_log_info(_mavlink_fd, "[mavlink] started"); @@ -1451,8 +1441,9 @@ Mavlink::task_main(int argc, char *argv[]) // mavlink_logbuffer_init(&lb, 10); int ch; - char *device_name = "/dev/ttyS1"; + const char *device_name = "/dev/ttyS1"; _baudrate = 57600; + _chan = MAVLINK_COMM_0; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -1587,7 +1578,7 @@ Mavlink::task_main(int argc, char *argv[]) /* arm counter to go off immediately */ unsigned lowspeed_counter = 10; - /* wakeup source(s) */ + /* wakeup source(s) */ struct pollfd fds[1]; /* Setup of loop */ @@ -1623,16 +1614,17 @@ Mavlink::task_main(int argc, char *argv[]) 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); + warnx("send heartbeat, chan: %d", _chan); + 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); + set_hil_enabled(true); else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); + set_hil_enabled(false); /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, + mavlink_msg_sys_status_send(_chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index bf8c63d389..8c9430829b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -182,7 +182,7 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool hil_enabled() { return _mavlink_hil_enabled; }; + bool get_hil_enabled() { return _mavlink_hil_enabled; }; /** * Handle waypoint related messages. @@ -206,7 +206,7 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_on_off(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); struct mavlink_subscriptions { int sensor_sub; @@ -242,7 +242,7 @@ public: struct mavlink_subscriptions subs; struct mavlink_subscriptions* get_subs() { return &subs; } - mavlink_channel_t get_chan() { return chan; } + mavlink_channel_t get_chan() { return _chan; } /** Global position */ struct vehicle_global_position_s global_pos; @@ -289,7 +289,7 @@ private: MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; - mavlink_channel_t chan; + mavlink_channel_t _chan; // XXX probably should be in a header... // extern pthread_t receive_start(int uart); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index ffff1838c9..6d64569de5 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -276,9 +276,9 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) /* enable or disable HIL */ if (l->listener->v_status.hil_state == HIL_STATE_ON) - l->mavlink->set_hil_on_off(true); + l->mavlink->set_hil_enabled(true); else if (l->listener->v_status.hil_state == HIL_STATE_OFF) - l->mavlink->set_hil_on_off(false); + l->mavlink->set_hil_enabled(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -464,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ - if (l->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + if (l->mavlink->get_hil_enabled() && l->listener->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; @@ -683,7 +683,6 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) struct listener* next = _listeners; unsigned i = 0; - while (next != nullptr) { fds[i].fd = *next->subp; @@ -708,7 +707,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) } else { - unsigned i = 0; + i = 0; struct listener* cb = _listeners; while (cb != nullptr) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3752bde104..94d97fc12c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -346,7 +346,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - if (_mavlink->hil_enabled()) { + if (_mavlink->get_hil_enabled()) { uint64_t timestamp = hrt_absolute_time(); From 19105bebc58a045446746813a50bf74faaa3ad39 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 16:16:57 +0100 Subject: [PATCH 15/99] Mavlink: hearbeat sending works now --- src/modules/mavlink/mavlink_main.cpp | 136 ++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 7 +- 2 files changed, 73 insertions(+), 70 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 21ba51b21c..cb620349d9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -135,9 +135,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - warnx("uart: %d", uart); - warnx("channel: %d", channel); - ssize_t desired = (sizeof(uint8_t) * length); ssize_t ret = write(uart, ch, desired); @@ -155,7 +152,7 @@ namespace mavlink } Mavlink::Mavlink() : - _mavlink_fd(-1), +// _mavlink_fd(-1), _task_should_exit(false), thread_running(false), _mavlink_task(-1), @@ -217,12 +214,18 @@ Mavlink* Mavlink::new_instance() { Mavlink* inst = new Mavlink(); Mavlink* next = ::_head; - while (next != nullptr) - next = next->_next; - - /* now parent has a null pointer, fill it */ - next = inst; + /* create the first instance at _head */ + if (::_head == nullptr) { + ::_head = inst; + /* afterwards follow the next and append the instance */ + } else { + while (next != nullptr) { + next = next->_next; + } + /* now parent has a null pointer, fill it */ + next = inst; + } return inst; } @@ -246,7 +249,7 @@ int Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); if (inst) - return inst->_mavlink_fd; + return inst->get_uart_fd(); return -1; } @@ -379,7 +382,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* open uart */ warnx("UART is %s, baudrate is %d\n", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); + _uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ struct termios uart_config; @@ -387,14 +390,14 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * *is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(_uart, uart_config_original)) < 0) { warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(uart); + close(_uart); return -1; } /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); + tcgetattr(_uart, &uart_config); /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -405,19 +408,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* 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); + close(_uart); return -1; } } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); + close(_uart); return -1; } - return uart; + return _uart; } int @@ -451,7 +454,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) } orb_set_interval(subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && _mavlink_hil_enabled) { @@ -541,50 +544,50 @@ Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_bas } -int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int Mavlink::set_mavlink_interval_limit(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); + 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); + 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); + 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); + 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(subs->rates_setpoint_sub, min_interval); + 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(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); + 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); + orb_set_interval(subs.debug_key_value, min_interval); break; default: @@ -1433,7 +1436,7 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize logging device */ // TODO - _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); +// _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); //mavlink_log_info(_mavlink_fd, "[mavlink] started"); @@ -1491,9 +1494,9 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* default values for arguments */ - uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); + _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) + if (_uart < 0) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ @@ -1516,57 +1519,57 @@ Mavlink::task_main(int argc, char *argv[]) /* all subscriptions are now active, set up initial guess about rate limits */ if (_baudrate >= 230400) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (_baudrate >= 115200) { /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (_baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -1581,6 +1584,8 @@ Mavlink::task_main(int argc, char *argv[]) /* wakeup source(s) */ struct pollfd fds[1]; + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; @@ -1614,7 +1619,6 @@ Mavlink::task_main(int argc, char *argv[]) get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - warnx("send heartbeat, chan: %d", _chan); mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ @@ -1688,7 +1692,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - tcsetattr(uart, TCSANOW, &uart_config_original); + tcsetattr(_uart, TCSANOW, &uart_config_original); /* destroy log buffer */ //mavlink_logbuffer_destroy(&lb); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8c9430829b..fb71128919 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -170,7 +170,7 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return uart; } + int get_uart_fd() { return _uart; } enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, @@ -265,7 +265,6 @@ protected: * registering clone devices etc. */ struct file_operations fops; - int _mavlink_fd; Mavlink* _next; private: @@ -302,7 +301,7 @@ private: mavlink_wpm_storage *wpm; bool verbose; - int uart; + int _uart; int _baudrate; bool gcs_link; /** @@ -390,7 +389,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval); /** * Callback for param interface. From ea2a69d8bf15cdb0c4a3234ed2851f9380d5adfc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 22:36:36 +0100 Subject: [PATCH 16/99] Mavlink: get orb_listener to work --- src/modules/mavlink/mavlink_main.cpp | 29 ++++++-- src/modules/mavlink/mavlink_orb_listener.cpp | 72 +++++++++++--------- src/modules/mavlink/mavlink_orb_listener.h | 6 +- src/modules/uORB/topics/vehicle_attitude.h | 2 +- 4 files changed, 65 insertions(+), 44 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb620349d9..f1ec6e8dce 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -153,6 +153,7 @@ namespace mavlink Mavlink::Mavlink() : // _mavlink_fd(-1), + _next(nullptr), _task_should_exit(false), thread_running(false), _mavlink_task(-1), @@ -1448,6 +1449,8 @@ Mavlink::task_main(int argc, char *argv[]) _baudrate = 57600; _chan = MAVLINK_COMM_0; + _mode = MODE_OFFBOARD; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; @@ -1488,7 +1491,23 @@ Mavlink::task_main(int argc, char *argv[]) warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ - warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + switch (_mode) { + case MODE_TX_HEARTBEAT_ONLY: + warnx("MODE_TX_HEARTBEAT_ONLY"); + 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; + } /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1506,12 +1525,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - // MavlinkReceiver rcv(this); - //receive_thread = MavlinkReceiver::receive_start(this); +// MavlinkReceiver rcv(this); + receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ - //MavlinkOrbListener listener(this); - //uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); +// MavlinkOrbListener listener(this); + uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 6d64569de5..418e1c1219 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -36,6 +36,7 @@ * Monitors ORB topics and sends update messages as appropriate. * * @author Lorenz Meier + * @author Julian Oes */ // XXX trim includes @@ -80,7 +81,7 @@ cm_uint16_from_m_float(float m) return (uint16_t)(m * 100.0f); } -MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : +MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), @@ -88,31 +89,31 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : _listeners(nullptr), _n_listeners(0) { - add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0); - add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0); - add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0); - add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0); - add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0); - add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0); - add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0); - add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3); - add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0); - add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0); - add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0); - add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0); - add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0); - add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0); - add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0); - add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0); - add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0); + add_listener(MavlinkOrbListener::l_sensor_combined, &(_mavlink->get_subs()->sensor_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude, &(_mavlink->get_subs()->att_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0); + add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0); + add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0); + add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0); + add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0); + add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0); + add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0); + add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0); + add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0); + add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3); + add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0); + add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0); + add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0); + add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0); + add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0); + add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_sub), 0); + add_listener(MavlinkOrbListener::l_nav_cap, &(_mavlink->get_subs()->navigation_capabilities_sub), 0); } @@ -124,15 +125,18 @@ void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l) nl->subp = subp; nl->arg = arg; nl->next = nullptr; + nl->mavlink = _mavlink; + nl->listener = this; - // Register it - struct listener *next = _listeners; - while (next->next != nullptr) { - next = next->next; + if (_listeners == nullptr) { + _listeners = nl; + } else { + struct listener *next = _listeners; + while (next->next != nullptr) { + next = next->next; + } + next->next = nl; } - - // Attach - next->next = nl; _n_listeners++; } @@ -271,7 +275,7 @@ void MavlinkOrbListener::l_vehicle_status(const struct listener *l) { /* immediately communicate state _mavlink->get_chan()ges back to user */ - orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status); + orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status)); orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); /* enable or disable HIL */ diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index c9f35a1fbd..349a11f2e9 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -36,6 +36,7 @@ * MAVLink 1.0 protocol interface definition. * * @author Lorenz Meier + * @author Julian Oes */ #include @@ -106,7 +107,7 @@ public: void (*callback)(const struct listener *l); int *subp; uintptr_t arg; - struct listener* next; + struct listener *next; Mavlink *mavlink; MavlinkOrbListener* listener; }; @@ -172,9 +173,6 @@ private: struct airspeed_s airspeed; struct home_position_s home; - int status_sub; - int rc_sub; - unsigned int sensors_raw_counter; unsigned int attitude_counter; unsigned int gps_counter; diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index e5a35ff9bc..40328af146 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 */ From 99b426c27c004b6430c4058151b0dd3854989414 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 12:38:35 +0100 Subject: [PATCH 17/99] Mavlink: bring mavlink log messages to life --- src/include/mavlink/mavlink_log.h | 3 ++ src/modules/mavlink/mavlink_main.cpp | 56 +++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 5 +-- 3 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 5054937e07..0ea655cac3 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/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f1ec6e8dce..4f4d949476 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,6 +90,12 @@ static const int ERROR = -1; static Mavlink* _head = nullptr; +/* TODO: if this is a class member it crashes */ +static struct file_operations fops; + +static struct mavlink_logbuffer lb; +static unsigned int total_counter; + /** * mavlink app start / stop handling function * @@ -152,7 +158,7 @@ namespace mavlink } Mavlink::Mavlink() : -// _mavlink_fd(-1), + _mavlink_fd(-1), _next(nullptr), _task_should_exit(false), thread_running(false), @@ -277,14 +283,15 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) 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_logbuffer_write(&lb, &msg); - //total_counter++; - return OK; - } + + const char *txt = (const char *)arg; +// printf("logmsg: %s\n", txt); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); + total_counter++; + return OK; + } default: return ENOTTY; @@ -1435,14 +1442,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("Initializing.."); fflush(stdout); - /* initialize logging device */ - // TODO -// _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); - - //mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* initialize mavlink text message buffering */ - // mavlink_logbuffer_init(&lb, 10); + mavlink_logbuffer_init(&lb, 10); int ch; const char *device_name = "/dev/ttyS1"; @@ -1519,7 +1520,12 @@ Mavlink::task_main(int argc, char *argv[]) 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); + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + + /* initialize logging device */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* Initialize system properties */ mavlink_update_system(); @@ -1693,15 +1699,15 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_pm_queued_send(); } - // /* send one string at 10 Hz */ - // if (!mavlink_logbuffer_is_empty(&lb)) { - // struct mavlink_logmessage msg; - // int lb_ret = mavlink_logbuffer_read(&lb, &msg); + /* 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); - // } - // } + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } perf_end(_loop_perf); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fb71128919..9b289c9658 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -264,11 +264,11 @@ protected: * Pointer to the default cdev file operations table; useful for * registering clone devices etc. */ - struct file_operations fops; + Mavlink* _next; private: - + int _mavlink_fd; bool _task_should_exit; /**< if true, mavlink task should exit */ bool thread_running; int _mavlink_task; /**< task handle for sensor task */ @@ -311,7 +311,6 @@ private: */ unsigned int mavlink_param_queue_index; - struct mavlink_logbuffer lb; bool mavlink_link_termination_allowed; /** From 76ae004e5c6c1dcc05f1eb784f6dc14cff2a3671 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 16:21:50 +0100 Subject: [PATCH 18/99] Defconfig: allow for more file descriptors --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 1dc96b3c3f..2e82b8a2d9 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=64 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 2a734c27e5..0508726777 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=64 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From cc5756f61ffa9b84abcbebfcf89d213d99d100d9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 18:35:45 +0100 Subject: [PATCH 19/99] Mavlink: enable log messages to multiple UARTs --- src/modules/mavlink/mavlink_main.cpp | 20 ++++++++++++-------- src/modules/mavlink/mavlink_main.h | 3 +++ 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2585593eaa..7e146604b7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -93,9 +93,6 @@ static Mavlink* _head = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; -static struct mavlink_logbuffer lb; -static unsigned int total_counter; - /** * mavlink app start / stop handling function * @@ -227,11 +224,11 @@ Mavlink* Mavlink::new_instance() ::_head = inst; /* afterwards follow the next and append the instance */ } else { - while (next != nullptr) { + while (next->_next != nullptr) { next = next->_next; } /* now parent has a null pointer, fill it */ - next = inst; + next->_next = inst; } return inst; } @@ -288,8 +285,15 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) // printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; + + Mavlink* inst = ::_head; + while (inst != nullptr) { + + mavlink_logbuffer_write(&inst->lb, &msg); + inst->total_counter++; + inst = inst->_next; + + } return OK; } @@ -1451,7 +1455,7 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 10); + mavlink_logbuffer_init(&lb, 5); int ch; const char *device_name = "/dev/ttyS1"; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ce2d2b34a2..b812af95a8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -293,6 +293,9 @@ private: // XXX probably should be in a header... // extern pthread_t receive_start(int uart); + struct mavlink_logbuffer lb; + unsigned int total_counter; + pthread_t receive_thread; pthread_t uorb_receive_thread; From 61a849bf6ba11b98b7332f89d0b32226601f4d63 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 19:13:57 +0100 Subject: [PATCH 20/99] Mavlink: don't allow multiple instances on the same device --- src/modules/mavlink/mavlink_main.cpp | 19 ++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 4 ++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7e146604b7..cbbe3c31f7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -155,6 +155,7 @@ namespace mavlink } Mavlink::Mavlink() : + device_name("/dev/ttyS1"), _mavlink_fd(-1), _next(nullptr), _task_should_exit(false), @@ -249,6 +250,19 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +bool Mavlink::instance_exists(const char *device_name, Mavlink *self) +{ + Mavlink* inst = ::_head; + 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); @@ -1458,7 +1472,6 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&lb, 5); int ch; - const char *device_name = "/dev/ttyS1"; _baudrate = 57600; _chan = MAVLINK_COMM_0; @@ -1496,6 +1509,10 @@ Mavlink::task_main(int argc, char *argv[]) } } + if (Mavlink::instance_exists(device_name, this)) { + errx(1, "mavlink instance for %s already running", device_name); + } + struct termios uart_config_original; bool usb_uart; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b812af95a8..bf7675267c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -168,10 +168,14 @@ public: static Mavlink* get_instance(unsigned instance); + static bool instance_exists(const char *device_name, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd() { return _uart; } + const char *device_name; + enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, MODE_OFFBOARD, From 346ae5b9f4fc2da13e6d890521f48768b6b6e8c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 19:13:10 +0100 Subject: [PATCH 21/99] Mavlink: allow to stop (WIP) --- src/modules/mavlink/mavlink_main.cpp | 62 +++++++++++++++----- src/modules/mavlink/mavlink_main.h | 5 +- src/modules/mavlink/mavlink_orb_listener.cpp | 3 +- src/modules/mavlink/mavlink_orb_listener.h | 2 - src/modules/mavlink/mavlink_receiver.cpp | 5 +- src/modules/mavlink/mavlink_receiver.h | 2 - 6 files changed, 54 insertions(+), 25 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cbbe3c31f7..0d9d061900 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -250,6 +250,43 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +int Mavlink::destroy_all_instances() +{ + /* start deleting from the end */ + Mavlink *inst_to_del = nullptr; + Mavlink *next_inst = ::_head; + + 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->thread_running) { + printf("."); + usleep(10000); + iterations++; + + if (iterations > 10000) { + warnx("ERROR: Couldn't stop all mavlink instances."); + return ERROR; + } + } + delete inst_to_del; + } + + /* reset head */ + ::_head = nullptr; + + printf("\n"); + warnx("all instances stopped"); + return OK; +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; @@ -1495,9 +1532,9 @@ Mavlink::task_main(int argc, char *argv[]) device_name = optarg; break; - case 'e': - mavlink_link_termination_allowed = true; - break; +// case 'e': +// mavlink_link_termination_allowed = true; +// break; case 'o': _mode = MODE_ONBOARD; @@ -1749,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[]) tcsetattr(_uart, TCSANOW, &uart_config_original); /* destroy log buffer */ - //mavlink_logbuffer_destroy(&lb); + mavlink_logbuffer_destroy(&lb); thread_running = false; @@ -1782,7 +1819,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop|status}"); + errx(1, "usage: mavlink {start|stop|status} [-d device] [-b baudrate] [-o]"); } int mavlink_main(int argc, char *argv[]) @@ -1830,21 +1867,16 @@ int mavlink_main(int argc, char *argv[]) // } return 0; - } - // if (mavlink::g_mavlink == nullptr) - // errx(1, "not running"); - - // if (!strcmp(argv[1], "stop")) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; + } else if (!strcmp(argv[1], "stop")) { + return Mavlink::destroy_all_instances(); // } else if (!strcmp(argv[1], "status")) { // mavlink::g_mavlink->status(); - // } else { - // usage(); - // } + } else { + usage(); + } return 0; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index bf7675267c..d5bbb746b9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -168,6 +168,8 @@ public: static Mavlink* get_instance(unsigned instance); + static int destroy_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static int get_uart_fd(unsigned index); @@ -263,6 +265,8 @@ public: /** Position setpoint triplet */ struct position_setpoint_triplet_s pos_sp_triplet; + bool _task_should_exit; /**< if true, mavlink task should exit */ + protected: /** * Pointer to the default cdev file operations table; useful for @@ -273,7 +277,6 @@ protected: private: int _mavlink_fd; - bool _task_should_exit; /**< if true, mavlink task should exit */ bool thread_running; int _mavlink_task; /**< task handle for sensor task */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index cd9408f2ff..a2b71c931c 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -83,7 +83,6 @@ cm_uint16_from_m_float(float m) MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), _listeners(nullptr), @@ -678,7 +677,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); - while (!thread_should_exit) { + while (!_mavlink->_task_should_exit) { int poll_ret = poll(fds, _n_listeners, timeout); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 560b474230..317a489d4b 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -116,8 +116,6 @@ public: private: - bool thread_should_exit; /**< if true, sensor task should exit */ - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 89dcb1d7bb..16a1b9aff6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,8 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0), - thread_should_exit(false) + alt0(0) { } @@ -807,7 +806,7 @@ MavlinkReceiver::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)) { /* to avoid reading very small chunks wait for data before reading */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d8d3b54529..6614e13f45 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -101,8 +101,6 @@ public: private: - bool thread_should_exit; /**< if true, sensor task should exit */ - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; From ef46cd5e909115c8c208c409fcded0dd02037d5e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 20:54:10 +0100 Subject: [PATCH 22/99] Mavlink: allow to stop (compiling, working) --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 16a1b9aff6..44fff24166 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -129,7 +129,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) usleep(50000); /* terminate other threads and this thread */ - thread_should_exit = true; + _mavlink->_task_should_exit = true; } else { From 523637e0f1fb0247111818d0a88ce8c4574728ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 13:36:59 +0100 Subject: [PATCH 23/99] Mavlink: Start multiple uart listeners, HIL working --- src/modules/mavlink/mavlink_main.cpp | 41 +++++++++++---- src/modules/mavlink/mavlink_main.h | 55 +++++++------------- src/modules/mavlink/mavlink_orb_listener.cpp | 12 ++--- src/modules/mavlink/mavlink_orb_listener.h | 23 +------- src/modules/mavlink/mavlink_receiver.cpp | 16 +++--- src/modules/mavlink/mavlink_receiver.h | 3 +- 6 files changed, 68 insertions(+), 82 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d9d061900..79aa6135b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -309,6 +309,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } +int Mavlink::get_uart_fd() +{ + return _uart; +} + +int Mavlink::get_channel() +{ + return (int)_chan; +} + void Mavlink::parameters_update() { @@ -945,7 +955,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + 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); @@ -968,7 +978,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = seq; - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + 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); @@ -988,7 +998,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_component = compid; wpc.count = mission.count; - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + 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); @@ -1018,7 +1028,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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_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); @@ -1036,7 +1046,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u 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_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); @@ -1061,7 +1071,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + 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); @@ -1318,7 +1328,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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) { + if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { wpm->timestamp_lastaction = now; @@ -1576,6 +1586,21 @@ Mavlink::task_main(int argc, char *argv[]) break; } + switch(_mode) { + case MODE_OFFBOARD: + case MODE_HIL: + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + break; + case MODE_ONBOARD: + _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; + break; + case MODE_TX_HEARTBEAT_ONLY: + default: + _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; + warnx("Error: Unknown mode"); + break; + } + /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1597,11 +1622,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ -// MavlinkReceiver rcv(this); receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ -// MavlinkOrbListener listener(this); uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d5bbb746b9..c667a41daa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -96,46 +96,27 @@ enum MAVLINK_WPM_CODES { }; -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 15 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#ifndef MAVLINK_WPM_TEXT_FEEDBACK -#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text -#endif -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_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 { - mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t size; - uint16_t max_size; - uint16_t rcv_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; - uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; - int current_dataman_id; + 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 { @@ -174,7 +155,9 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return _uart; } + int get_uart_fd(); + + int get_channel(); const char *device_name; @@ -294,7 +277,7 @@ private: uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER MAVLINK_MODE _mode; - uint8_t mavlink_wpm_comp_id; + uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _chan; // XXX probably should be in a header... diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index a2b71c931c..fdc196371b 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -55,14 +55,14 @@ #include #include #include +#include + #include #include "mavlink_orb_listener.h" #include "mavlink_main.h" - void *uorb_receive_thread(void *arg); - uint16_t cm_uint16_from_m_float(float m); @@ -647,9 +647,9 @@ void * MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ - char buf[32]; - sprintf(buf, "mavlink rcv%d", Mavlink::instance_count()); - prctl(PR_SET_NAME, buf, getpid()); + char thread_name[18]; + sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); /* * set up poll to block for new data, @@ -822,7 +822,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink); pthread_attr_destroy(&uorb_attr); return thread; diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 317a489d4b..26a2e832f7 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -87,20 +87,7 @@ public: */ ~MavlinkOrbListener(); - // * - // * Start the mavlink task. - // * - // * @return OK on success. - - // int start(); - - /** - * Display the mavlink status. - */ - void status(); - static pthread_t uorb_receive_start(Mavlink *mavlink); - void * uorb_receive_thread(void *arg); struct listener { void (*callback)(const struct listener *l); @@ -124,15 +111,7 @@ private: unsigned _n_listeners; static const unsigned _max_listeners = 32; - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - - /** - * Main sensor collection task. - */ - void task_main() __attribute__((noreturn)); + void *uorb_receive_thread(void *arg); static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 44fff24166..371f945c4c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -798,7 +798,10 @@ MavlinkReceiver::receive_thread(void *arg) 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()); struct pollfd fds[1]; fds[0].fd = uart_fd; @@ -842,20 +845,19 @@ void MavlinkReceiver::print_status() void * MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context)); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); return rcv->receive_thread(NULL); } pthread_t -MavlinkReceiver::receive_start(Mavlink *mavlink) +MavlinkReceiver::receive_start(Mavlink *parent) { - pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); // set to non-blocking read - int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0); - fcntl(mavlink->get_uart_fd(), 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); @@ -865,7 +867,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 6614e13f45..8e139e5e41 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -95,7 +95,7 @@ public: */ void print_status(); - static pthread_t receive_start(Mavlink* mavlink); + static pthread_t receive_start(Mavlink *parent); static void * start_helper(void *context); @@ -105,7 +105,6 @@ private: Mavlink* _mavlink; - void handle_message(mavlink_message_t *msg); void *receive_thread(void *arg); From 71cd1326635bead6c788b8967c6d5bcfd920b49c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 14:24:26 +0100 Subject: [PATCH 24/99] Mavlink: Don't support mavlink stop so that QGC doesn't stop mavlink before it starts rc.usb --- src/modules/mavlink/mavlink_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 79aa6135b3..d2c94ca92b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1842,7 +1842,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop|status} [-d device] [-b baudrate] [-o]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]"); } int mavlink_main(int argc, char *argv[]) @@ -1892,6 +1892,10 @@ int mavlink_main(int argc, char *argv[]) return 0; } 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")) { From 30dacbd1541073f9e35b8de0d1410e23f3aaccda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Feb 2014 11:29:36 +0100 Subject: [PATCH 25/99] Mavlink and navigator: handle current waypoint onboard --- src/modules/mavlink/mavlink_main.cpp | 102 +++++++++++--------- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/navigator/navigator_main.cpp | 25 +++-- src/modules/navigator/navigator_mission.cpp | 67 +++++++++++-- src/modules/navigator/navigator_mission.h | 11 ++- src/modules/uORB/topics/mission_result.h | 3 +- 6 files changed, 142 insertions(+), 68 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d2c94ca92b..38f753c396 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -59,11 +59,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -958,7 +958,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 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); + if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } /* @@ -981,11 +981,11 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t 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); + 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"); + if (_verbose) warnx("ERROR: index out of bounds"); } } @@ -1001,7 +1001,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin 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); + 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) @@ -1031,10 +1031,10 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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); + 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); + if (_verbose) warnx("ERROR: could not read WP%u", seq); } } @@ -1049,11 +1049,11 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u 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); + 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"); + if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity"); } } @@ -1074,10 +1074,9 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t 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); + if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); } - void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ @@ -1085,7 +1084,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) 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); + 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; @@ -1117,7 +1116,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); + if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch"); } break; @@ -1140,18 +1139,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (verbose) warnx("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"); + 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"); + if (_verbose) warnx("REJ. WP CMD: target id mismatch"); } break; @@ -1173,7 +1172,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) wpm->current_partner_compid = msg->compid; } else { - if (verbose) warnx("No waypoints send"); + if (_verbose) warnx("No waypoints send"); } wpm->current_count = wpm->size; @@ -1181,11 +1180,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (verbose) warnx("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"); + if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch"); } break; @@ -1201,7 +1200,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); break; } @@ -1212,11 +1211,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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"); + if (_verbose) warnx("REJ. WP CMD: First id != 0"); break; } @@ -1224,22 +1223,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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); + 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); + 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); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); break; } @@ -1252,7 +1251,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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); + if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } @@ -1261,12 +1260,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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"); + if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1282,18 +1281,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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"); + 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); + 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; @@ -1307,19 +1306,19 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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); + 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"); + 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"); + if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1389,15 +1388,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - if (wp.current) { - mission.current_index = wp.seq; - } +// 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); + 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); @@ -1416,7 +1418,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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"); + if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; @@ -1448,14 +1450,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (verbose) warnx("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"); + if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; @@ -1550,6 +1552,9 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MODE_ONBOARD; break; + case 'v': + _verbose = true; + default: usage(); break; @@ -1767,9 +1772,12 @@ Mavlink::task_main(int argc, char *argv[]) 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); - } + if (_verbose) warnx("Got mission result"); + + 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); } mavlink_waypoint_eventloop(hrt_absolute_time()); @@ -1842,7 +1850,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c667a41daa..9461a51f8f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -293,7 +293,7 @@ private: mavlink_wpm_storage wpm_s; mavlink_wpm_storage *wpm; - bool verbose; + bool _verbose; int _uart; int _baudrate; bool gcs_link; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 260356eca9..e1dde35bff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -65,7 +65,6 @@ #include #include #include -#include #include #include #include @@ -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); @@ -384,7 +383,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), - _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -416,7 +414,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)); @@ -518,13 +515,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 @@ -534,12 +534,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); } } @@ -1116,6 +1116,9 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + + _mission.report_current_mission_item(); + /* reset time counter for new item */ _time_first_inside_orbit = 0; @@ -1537,6 +1540,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; @@ -1589,6 +1595,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 e72caf98e9..fdc4ffff69 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 -// #include -// #include -// #include - +#include #include #include +#include +#include +#include #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_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,37 @@ 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_mission_item() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _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 15d4e86bfb..845c16583d 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -40,6 +40,7 @@ #define NAVIGATOR_MISSION_H #include +#include 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_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 c99706b97a..7c3921198a 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 */ }; /** From b596bf6aa56b2039b51a03ab11acf2d7d719195d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 13:24:02 +0100 Subject: [PATCH 26/99] Mavlink: gotten rid of some warnings --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++++----- src/modules/mavlink/mavlink_receiver.cpp | 13 +++++-------- src/modules/mavlink/mavlink_receiver.h | 2 +- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 38f753c396..42504dea9f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -52,7 +52,12 @@ #include #include #include +#include /* isinf / isnan checks */ + #include +#include +#include + #include #include #include @@ -64,6 +69,7 @@ #include #include #include + #include #include #include @@ -72,12 +78,10 @@ #include #include #include -#include -#include + #include #include "mavlink_bridge_header.h" -#include "math.h" /* isinf / isnan checks */ #include "mavlink_main.h" #include "mavlink_orb_listener.h" #include "mavlink_receiver.h" @@ -156,9 +160,9 @@ namespace mavlink Mavlink::Mavlink() : device_name("/dev/ttyS1"), - _mavlink_fd(-1), - _next(nullptr), _task_should_exit(false), + _next(nullptr), + _mavlink_fd(-1), thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 371f945c4c..d07de0f22f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0) + alt0(0.0) { } @@ -605,8 +605,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - 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); @@ -628,7 +626,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (pub_hil_local_pos > 0) { float x; float y; - bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? double lat = hil_state.lat*1e-7; double lon = hil_state.lon*1e-7; map_projection_project(lat, lon, &x, &y); @@ -811,14 +809,13 @@ MavlinkReceiver::receive_thread(void *arg) 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(_mavlink->get_chan(), buf[i], &msg, &status)) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8e139e5e41..fca5de9179 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -140,6 +140,6 @@ private: orb_advert_t telemetry_status_pub; int32_t lat0; int32_t lon0; - double alt0; + float alt0; }; From 366f19c2c07f249a6ac51f16f08b86454e2e2da4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 13:24:56 +0100 Subject: [PATCH 27/99] Stack: lower stack of lpwork and hpwork --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 2e82b8a2d9..7aa71f1826 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -720,11 +720,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/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 0508726777..d42b23ab0b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -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 From 73edc020162841f250cf8ba26c6bdc40837a0e4f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Feb 2014 13:42:15 +0100 Subject: [PATCH 28/99] mavlink: fix verbose mode and actually publish mission --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 42504dea9f..15f2fd2ca4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -169,8 +169,10 @@ Mavlink::Mavlink() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), - _mavlink_hil_enabled(false) + _mavlink_hil_enabled(false), // _params_sub(-1) + + mission_pub(-1) { wpm = &wpm_s; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; @@ -987,6 +989,10 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); + } 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"); @@ -1534,7 +1540,7 @@ Mavlink::task_main(int argc, char *argv[]) argc -= 2; argv += 2; - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1558,6 +1564,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'v': _verbose = true; + break; default: usage(); @@ -1778,8 +1785,9 @@ Mavlink::task_main(int argc, char *argv[]) if (_verbose) warnx("Got mission result"); - if (mission_result.mission_reached) + 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); } From 926c4701c71fb2689025decbc454d14c6df85e76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:17:13 +0100 Subject: [PATCH 29/99] mavlink: set current WP working as expected, report current WP with 0.5 Hz --- src/modules/mavlink/mavlink_main.cpp | 19 +++++++++++-------- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_mission.cpp | 8 +++----- src/modules/navigator/navigator_mission.h | 2 +- 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 15f2fd2ca4..1ce467a7b8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -987,8 +987,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t 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 if (seq == 0 && wpm->size == 0) { /* don't broadcast if no WPs */ @@ -1145,7 +1143,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - mavlink_wpm_send_waypoint_current(wpc.seq); + /* 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"); @@ -1708,8 +1707,7 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - /* arm counter to go off immediately */ - unsigned lowspeed_counter = 10; + unsigned lowspeed_counter = 0; /* wakeup source(s) */ struct pollfd fds[1]; @@ -1739,7 +1737,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* 1 Hz */ - if (lowspeed_counter == 10) { + if (lowspeed_counter % 10 == 0) { mavlink_update_system(); /* translate the current system state to mavlink state and mode */ @@ -1772,7 +1770,12 @@ Mavlink::task_main(int argc, char *argv[]) v_status.errors_count2, v_status.errors_count3, v_status.errors_count4); - lowspeed_counter = 0; + } + + /* 0.5 Hz */ + if (lowspeed_counter % 20 == 0) { + + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } lowspeed_counter++; @@ -1783,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - if (_verbose) warnx("Got 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); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5559d7b562..4d135a0ef0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1105,7 +1105,7 @@ Navigator::set_mission_item() if (ret == OK) { - _mission.report_current_mission_item(); + _mission.report_current_offboard_mission_item(); /* reset time counter for new item */ _time_first_inside_orbit = 0; diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index fdc4ffff69..72dddebfee 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -89,7 +89,7 @@ Mission::set_current_offboard_mission_index(int new_index) _current_offboard_mission_index = 0; } } - report_current_mission_item(); + report_current_offboard_mission_item(); } void @@ -296,11 +296,9 @@ Mission::report_mission_item_reached() } void -Mission::report_current_mission_item() +Mission::report_current_offboard_mission_item() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.index_current_mission = _current_offboard_mission_index; - } + _mission_result.index_current_mission = _current_offboard_mission_index; } void diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 845c16583d..2bd4da82eb 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -73,7 +73,7 @@ public: void move_to_next(); void report_mission_item_reached(); - void report_current_mission_item(); + void report_current_offboard_mission_item(); void publish_mission_result(); private: From d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Feb 2014 23:46:58 +0400 Subject: [PATCH 30/99] mavlink: major rewrite, prepare for dynamic mavlink streams configuration, WIP --- src/modules/mavlink/mavlink_orb_listener.cpp | 1192 +++++++---------- src/modules/mavlink/mavlink_orb_listener.h | 95 +- .../mavlink/mavlink_orb_subscription.cpp | 47 + .../mavlink/mavlink_orb_subscription.h | 31 + src/modules/mavlink/mavlink_stream.cpp | 48 + src/modules/mavlink/mavlink_stream.h | 33 + src/modules/mavlink/module.mk | 4 +- 7 files changed, 718 insertions(+), 732 deletions(-) create mode 100644 src/modules/mavlink/mavlink_orb_subscription.cpp create mode 100644 src/modules/mavlink/mavlink_orb_subscription.h create mode 100644 src/modules/mavlink/mavlink_stream.cpp create mode 100644 src/modules/mavlink/mavlink_stream.h diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index fdc196371b..ea1e3a8bb9 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -57,17 +57,14 @@ #include #include - #include #include "mavlink_orb_listener.h" #include "mavlink_main.h" - uint16_t cm_uint16_from_m_float(float m); - uint16_t cm_uint16_from_m_float(float m) { @@ -85,212 +82,195 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), - _listeners(nullptr), - _n_listeners(0) + _subscriptions(nullptr), + _streams(nullptr) { - add_listener(MavlinkOrbListener::l_sensor_combined, &(_mavlink->get_subs()->sensor_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude, &(_mavlink->get_subs()->att_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0); - add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0); - add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0); - add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0); - add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0); - add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0); - add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0); - add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0); - add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3); - add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0); - add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0); - add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0); - add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0); - add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0); - add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_sub), 0); - add_listener(MavlinkOrbListener::l_nav_cap, &(_mavlink->get_subs()->navigation_capabilities_sub), 0); } -void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg) +MavlinkOrbListener::~MavlinkOrbListener() { - struct listener *nl = new listener; - nl->callback = callback; - nl->subp = subp; - nl->arg = arg; - nl->next = nullptr; - nl->mavlink = _mavlink; - nl->listener = this; - - if (_listeners == nullptr) { - _listeners = nl; - } else { - struct listener *next = _listeners; - while (next->next != nullptr) { - next = next->next; - } - next->next = nl; - } - _n_listeners++; } -void -MavlinkOrbListener::l_sensor_combined(const struct listener *l) +MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval) { - struct sensor_combined_s raw; + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - - /* mark individual fields as _mavlink->get_chan()ged */ - uint16_t fields_updated = 0; - - // if (accel_counter != raw.accelerometer_counter) { - // /* mark first three dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - // accel_counter = raw.accelerometer_counter; - // } - - // if (gyro_counter != raw.gyro_counter) { - // /* mark second group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - // gyro_counter = raw.gyro_counter; - // } - - // if (mag_counter != raw.magnetometer_counter) { - // /* mark third group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - // mag_counter = raw.magnetometer_counter; - // } - - // if (baro_counter != raw.baro_counter) { - // /* mark last group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - // baro_counter = raw.baro_counter; - // } - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->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); - - l->listener->sensors_raw_counter++; -} - -void -MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) -{ - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - /* send sensor values */ - mavlink_msg_attitude_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - l->listener->att.roll, - l->listener->att.pitch, - l->listener->att.yaw, - l->listener->att.rollspeed, - l->listener->att.pitchspeed, - l->listener->att.yawspeed); - - /* limit VFR message rate to 10Hz */ - hrt_abstime t = hrt_absolute_time(); - if (t >= l->listener->last_sent_vfr + 100000) { - l->listener->last_sent_vfr = t; - float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); - uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; - float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); - } - - /* send quaternion values if it exists */ - if(l->listener->att.q_valid) { - mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - l->listener->att.q[0], - l->listener->att.q[1], - l->listener->att.q[2], - l->listener->att.q[3], - l->listener->att.rollspeed, - l->listener->att.pitchspeed, - l->listener->att.yawspeed); + LL_FOREACH(_subscriptions, sub) { + if (sub->meta == meta) { + /* already subscribed */ + if (sub->interval > interval) { + /* subscribed with bigger interval, change interval */ + sub->set_interval(interval); + } + return sub; } } - l->listener->attitude_counter++; + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(meta, size); + + sub_new->set_interval(interval); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; } -void -MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) +void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) { - struct vehicle_gps_position_s gps; + MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval); - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); + stream->mavlink = _mavlink; - /* 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(l->mavlink->get_chan(), - 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 && (l->listener->gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(l->mavlink->get_chan(), - gps.satellites_visible, - gps.satellite_prn, - gps.satellite_used, - gps.satellite_elevation, - gps.satellite_azimuth, - gps.satellite_snr); - } - - l->listener->gps_counter++; + LL_APPEND(_streams, stream); } +//void +//MavlinkOrbListener::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), l->mavlink->get_subs()->sensor_sub, &raw); +// +// /* mark individual fields as _mavlink->get_chan()ged */ +// uint16_t fields_updated = 0; +// +// // if (accel_counter != raw.accelerometer_counter) { +// // /* mark first three dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); +// // accel_counter = raw.accelerometer_counter; +// // } +// +// // if (gyro_counter != raw.gyro_counter) { +// // /* mark second group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); +// // gyro_counter = raw.gyro_counter; +// // } +// +// // if (mag_counter != raw.magnetometer_counter) { +// // /* mark third group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); +// // mag_counter = raw.magnetometer_counter; +// // } +// +// // if (baro_counter != raw.baro_counter) { +// // /* mark last group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); +// // baro_counter = raw.baro_counter; +// // } +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->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); +// +// l->listener->sensors_raw_counter++; +//} +// +//void +//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) +//{ +// /* copy attitude data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send sensor values */ +// mavlink_msg_attitude_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.roll, +// l->listener->att.pitch, +// l->listener->att.yaw, +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// +// /* limit VFR message rate to 10Hz */ +// hrt_abstime t = hrt_absolute_time(); +// if (t >= l->listener->last_sent_vfr + 100000) { +// l->listener->last_sent_vfr = t; +// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); +// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; +// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; +// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); +// } +// +// /* send quaternion values if it exists */ +// if(l->listener->att.q_valid) { +// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.q[0], +// l->listener->att.q[1], +// l->listener->att.q[2], +// l->listener->att.q[3], +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// } +// } +// +// l->listener->attitude_counter++; +//} +// +//void +//MavlinkOrbListener::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), l->mavlink->get_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(l->mavlink->get_chan(), +// 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 && (l->listener->gps_counter % 50 == 0)) { +// mavlink_msg_gps_status_send(l->mavlink->get_chan(), +// gps.satellites_visible, +// gps.satellite_prn, +// gps.satellite_used, +// gps.satellite_elevation, +// gps.satellite_azimuth, +// gps.satellite_snr); +// } +// +// l->listener->gps_counter++; +//} +// + void -MavlinkOrbListener::l_vehicle_status(const struct listener *l) +MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream) { - /* immediately communicate state _mavlink->get_chan()ges back to user */ - orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status)); - orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); - orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet)); - - /* enable or disable HIL */ - if (l->listener->v_status.hil_state == HIL_STATE_ON) - l->mavlink->set_hil_enabled(true); - else if (l->listener->v_status.hil_state == HIL_STATE_OFF) - l->mavlink->set_hil_enabled(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; - l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + //l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(l->mavlink->get_chan(), + mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -298,409 +278,378 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) mavlink_state); } -void -MavlinkOrbListener::l_rc_channels(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); - // XXX Add RC channels scaled message here -} - -void -MavlinkOrbListener::l_input_rc(const struct listener *l) -{ - /* copy rc _mavlink->get_chan()nels into local buffer */ - orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), - l->listener->rc_raw.timestamp_publication / 1000, - i, - (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, - l->listener->rc_raw.rssi); - } - } -} - -void -MavlinkOrbListener::l_global_position(const struct listener *l) -{ - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); - - mavlink_msg_global_position_int_send(l->mavlink->get_chan(), - l->listener->global_pos.timestamp / 1000, - l->listener->global_pos.lat * 1e7, - l->listener->global_pos.lon * 1e7, - l->listener->global_pos.alt * 1000.0f, - (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, - l->listener->global_pos.vel_n * 100.0f, - l->listener->global_pos.vel_e * 100.0f, - l->listener->global_pos.vel_d * 100.0f, - _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -} - -void -MavlinkOrbListener::l_local_position(const struct listener *l) -{ - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), - l->listener->local_pos.timestamp / 1000, - l->listener->local_pos.x, - l->listener->local_pos.y, - l->listener->local_pos.z, - l->listener->local_pos.vx, - l->listener->local_pos.vy, - l->listener->local_pos.vz); -} - -void -MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -{ - struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); - - if (!triplet.current.valid) - return; - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), - 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 -MavlinkOrbListener::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), l->mavlink->get_subs()->spl_sub, &local_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); -} - -void -MavlinkOrbListener::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), l->mavlink->get_subs()->spa_sub, &att_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); -} - -void -MavlinkOrbListener::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), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), - rates_sp.timestamp / 1000, - rates_sp.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); -} - -void -MavlinkOrbListener::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 (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->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 (l->mavlink->get_hil_enabled() && l->listener->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; - l->mavlink->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(l->mavlink->get_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(l->mavlink->get_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(l->mavlink->get_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(l->mavlink->get_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 -MavlinkOrbListener::l_actuator_armed(const struct listener *l) -{ - orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -} - -void -MavlinkOrbListener::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), l->mavlink->get_subs()->man_control_sp_sub, &man_control); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_manual_control_send(l->mavlink->get_chan(), - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); -} - -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_optical_flow(const struct listener *l) -{ - struct optical_flow_s flow; - - orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); - - mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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 -MavlinkOrbListener::l_home(const struct listener *l) -{ - orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); - - mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -} - -void -MavlinkOrbListener::l_airspeed(const struct listener *l) -{ - orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -} - -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); - -} +// +//void +//MavlinkOrbListener::l_rc_channels(const struct listener *l) +//{ +// /* copy rc channels into local buffer */ +// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); +// // XXX Add RC channels scaled message here +//} +// +//void +//MavlinkOrbListener::l_input_rc(const struct listener *l) +//{ +// /* copy rc _mavlink->get_chan()nels into local buffer */ +// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// +// const unsigned port_width = 8; +// +// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), +// l->listener->rc_raw.timestamp_publication / 1000, +// i, +// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, +// l->listener->rc_raw.rssi); +// } +// } +//} +// +//void +//MavlinkOrbListener::l_global_position(const struct listener *l) +//{ +// /* copy global position data into local buffer */ +// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); +// +// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), +// l->listener->global_pos.timestamp / 1000, +// l->listener->global_pos.lat * 1e7, +// l->listener->global_pos.lon * 1e7, +// l->listener->global_pos.alt * 1000.0f, +// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, +// l->listener->global_pos.vel_n * 100.0f, +// l->listener->global_pos.vel_e * 100.0f, +// l->listener->global_pos.vel_d * 100.0f, +// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +//} +// +//void +//MavlinkOrbListener::l_local_position(const struct listener *l) +//{ +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), +// l->listener->local_pos.timestamp / 1000, +// l->listener->local_pos.x, +// l->listener->local_pos.y, +// l->listener->local_pos.z, +// l->listener->local_pos.vx, +// l->listener->local_pos.vy, +// l->listener->local_pos.vz); +//} +// +//void +//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) +//{ +// struct position_setpoint_triplet_s triplet; +// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); +// +// if (!triplet.current.valid) +// return; +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), +// 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 +//MavlinkOrbListener::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), l->mavlink->get_subs()->spl_sub, &local_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), +// MAV_FRAME_LOCAL_NED, +// local_sp.x, +// local_sp.y, +// local_sp.z, +// local_sp.yaw); +//} +// +//void +//MavlinkOrbListener::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), l->mavlink->get_subs()->spa_sub, &att_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +//} +// +//void +//MavlinkOrbListener::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), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), +// rates_sp.timestamp / 1000, +// rates_sp.roll, +// rates_sp.pitch, +// rates_sp.yaw, +// rates_sp.thrust); +//} +// +//void +//MavlinkOrbListener::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 (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->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 (l->mavlink->get_hil_enabled() && l->listener->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; +// l->mavlink->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(l->mavlink->get_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(l->mavlink->get_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(l->mavlink->get_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(l->mavlink->get_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 +//MavlinkOrbListener::l_actuator_armed(const struct listener *l) +//{ +// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); +//} +// +//void +//MavlinkOrbListener::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), l->mavlink->get_subs()->man_control_sp_sub, &man_control); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_manual_control_send(l->mavlink->get_chan(), +// mavlink_system.sysid, +// man_control.roll * 1000, +// man_control.pitch * 1000, +// man_control.yaw * 1000, +// man_control.throttle * 1000, +// 0); +//} +// +//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_optical_flow(const struct listener *l) +//{ +// struct optical_flow_s flow; +// +// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); +// +// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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 +//MavlinkOrbListener::l_home(const struct listener *l) +//{ +// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); +// +// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); +//} +// +//void +//MavlinkOrbListener::l_airspeed(const struct listener *l) +//{ +// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); +//} +// +//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); +// +//} void * MavlinkOrbListener::uorb_receive_thread(void *arg) { - /* Set thread name */ + /* set thread name */ char thread_name[18]; sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); prctl(PR_SET_NAME, thread_name, getpid()); - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; + /* add mavlink streams */ + /* common buffer for topics and data sizes */ + const struct orb_metadata *topics[1]; + size_t sizes[1]; - /* - * Initialise listener array. - * - * Might want to invoke each listener once to set initial state. - */ - struct pollfd fds[_max_listeners]; - - struct listener* next = _listeners; - unsigned i = 0; - while (next != nullptr) { - - fds[i].fd = *next->subp; - fds[i].events = POLLIN; - - next = next->next; - i++; - } - /* Invoke callback to set initial state */ - //listeners[i].callback(&listener[i]); + /* --- HEARTBEAT --- */ + topics[0] = ORB_ID(vehicle_status); + sizes[0] = sizeof(vehicle_status_s); + add_stream(msg_heartbeat, 1, topics, sizes, 0, 500); while (!_mavlink->_task_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 { - - i = 0; - struct listener* cb = _listeners; - while (cb != nullptr) { - - if (fds[i].revents & POLLIN) - cb->callback(cb); - - cb = cb->next; - i++; - } + /* check all streams each 1ms */ + hrt_abstime t = hrt_absolute_time(); + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); } + usleep(1000); } return NULL; @@ -715,105 +664,6 @@ void * MavlinkOrbListener::uorb_start_helper(void *context) pthread_t MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) { - - /* --- SENSORS RAW VALUE --- */ - mavlink->get_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->get_subs()->sensor_sub, 200); /* 5Hz updates */ - - /* --- ATTITUDE VALUE --- */ - mavlink->get_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->get_subs()->att_sub, 200); /* 5Hz updates */ - - /* --- GPS VALUE --- */ - mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink->get_subs()->gps_sub, 200); /* 5Hz updates */ - - /* --- HOME POSITION --- */ - mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position)); - orb_set_interval(mavlink->get_subs()->home_sub, 1000); /* 1Hz updates */ - - /* --- SYSTEM STATE --- */ - mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ - - /* --- POSITION SETPOINT TRIPLET --- */ - mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0); /* not polled, don't limit */ - - /* --- RC CHANNELS VALUE --- */ - mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(mavlink->get_subs()->rc_sub, 100); /* 10Hz updates */ - - /* --- RC RAW VALUE --- */ - mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc)); - orb_set_interval(mavlink->get_subs()->input_rc_sub, 100); - - /* --- GLOBAL POS VALUE --- */ - mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink->get_subs()->global_pos_sub, 100); /* 10 Hz active updates */ - - /* --- LOCAL POS VALUE --- */ - mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */ - - /* --- GLOBAL SETPOINT VALUE --- */ - mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink->get_subs()->triplet_sub, 2000); /* 0.5 Hz updates */ - - /* --- LOCAL SETPOINT VALUE --- */ - mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(mavlink->get_subs()->spl_sub, 2000); /* 0.5 Hz updates */ - - /* --- ATTITUDE SETPOINT VALUE --- */ - mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(mavlink->get_subs()->spa_sub, 2000); /* 0.5 Hz updates */ - - /* --- RATES SETPOINT VALUE --- */ - mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000); /* 0.5 Hz updates */ - - /* --- ACTUATOR OUTPUTS --- */ - mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - mavlink->get_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->get_subs()->act_0_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_3_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR ARMED VALUE --- */ - mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */ - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink->get_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->get_subs()->man_control_sp_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR CONTROL VALUE --- */ - mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(mavlink->get_subs()->actuators_sub, 100); /* 10Hz updates */ - - /* --- DEBUG VALUE OUTPUT --- */ - mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - orb_set_interval(mavlink->get_subs()->debug_key_value, 100); /* 10Hz updates */ - - /* --- FLOW SENSOR --- */ - mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow)); - orb_set_interval(mavlink->get_subs()->optical_flow, 200); /* 5Hz updates */ - - /* --- AIRSPEED --- */ - mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); /* 5Hz updates */ - - /* --- NAVIGATION CAPABILITIES --- */ - mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */ - /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 26a2e832f7..1a103e43d3 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -40,9 +40,12 @@ */ #include +#include #pragma once +#include + #include #include #include @@ -68,11 +71,14 @@ #include #include #include -#include #include #include +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" + class Mavlink; +class MavlinkStream; class MavlinkOrbListener { @@ -83,22 +89,14 @@ public: MavlinkOrbListener(Mavlink* parent); /** - * Destructor, also kills the mavlinks task. + * Destructor, closes all subscriptions. */ ~MavlinkOrbListener(); static pthread_t uorb_receive_start(Mavlink *mavlink); - struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; - struct listener *next; - Mavlink *mavlink; - MavlinkOrbListener* listener; - }; - - void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg); + MavlinkOrbSubscription *add_subscription(const struct orb_metadata *meta, size_t size, const MavlinkStream *stream, const unsigned int interval); + void add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); static void * uorb_start_helper(void *context); private: @@ -107,57 +105,34 @@ private: Mavlink* _mavlink; - struct listener *_listeners; - unsigned _n_listeners; - static const unsigned _max_listeners = 32; + MavlinkOrbSubscription *_subscriptions; + static const unsigned _max_subscriptions = 32; + MavlinkStream *_streams; void *uorb_receive_thread(void *arg); - static void l_sensor_combined(const struct listener *l); - static void l_vehicle_attitude(const struct listener *l); - static void l_vehicle_gps_position(const struct listener *l); - static void l_vehicle_status(const struct listener *l); - static void l_rc_channels(const struct listener *l); - static void l_input_rc(const struct listener *l); - static void l_global_position(const struct listener *l); - static void l_local_position(const struct listener *l); - static void l_global_position_setpoint(const struct listener *l); - static void l_local_position_setpoint(const struct listener *l); - static void l_attitude_setpoint(const struct listener *l); - static void l_actuator_outputs(const struct listener *l); - static void l_actuator_armed(const struct listener *l); - static void l_manual_control_setpoint(const struct listener *l); - static void l_vehicle_attitude_controls(const struct listener *l); - static void l_debug_key_value(const struct listener *l); - static void l_optical_flow(const struct listener *l); - static void l_vehicle_rates_setpoint(const struct listener *l); - static void l_home(const struct listener *l); - static void l_airspeed(const struct listener *l); - static void l_nav_cap(const struct listener *l); + static void msg_heartbeat(const MavlinkStream *stream); - struct vehicle_global_position_s global_pos; - struct vehicle_local_position_s local_pos; - struct navigation_capabilities_s nav_cap; - struct vehicle_status_s v_status; - 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 home_position_s home; - - unsigned int sensors_raw_counter; - unsigned int attitude_counter; - unsigned int gps_counter; - - /* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ - uint64_t last_sensor_timestamp; - - hrt_abstime last_sent_vfr; +// 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); }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp new file mode 100644 index 0000000000..182407a5eb --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -0,0 +1,47 @@ +/* + * mavlink_orb_subscription.cpp + * + * Created on: 23.02.2014 + * Author: ton + */ + + +#include +#include +#include +#include + +#include "mavlink_orb_subscription.h" + +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size) +{ + this->meta = meta; + this->data = malloc(size); + memset(this->data, 0, size); + this->fd = orb_subscribe(meta); + this->last_update = 0; + this->interval = 0; +} + +MavlinkOrbSubscription::~MavlinkOrbSubscription() +{ + close(fd); + free(data); +} + +int MavlinkOrbSubscription::set_interval(const unsigned int interval) +{ + this->interval = interval; + return orb_set_interval(fd, interval); +} + +int MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (last_update != t) { + bool updated; + orb_check(fd, &updated); + if (updated) + return orb_copy(meta, fd, &data); + } + return OK; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h new file mode 100644 index 0000000000..2c72abaeff --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -0,0 +1,31 @@ +/* + * mavlink_orb_subscription.h + * + * Created on: 23.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_ORB_SUBSCRIPTION_H_ +#define MAVLINK_ORB_SUBSCRIPTION_H_ + +#include +#include + +class MavlinkOrbSubscription { +public: + MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); + ~MavlinkOrbSubscription(); + + int set_interval(const unsigned int interval); + int update(const hrt_abstime t); + + const struct orb_metadata *meta; + int fd; + void *data; + hrt_abstime last_update; + unsigned int interval; + MavlinkOrbSubscription *next; +}; + + +#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */ diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp new file mode 100644 index 0000000000..2a6728fdb2 --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -0,0 +1,48 @@ +/* + * mavlink_stream.cpp + * + * Created on: 24.02.2014 + * Author: ton + */ + +#include + +#include "mavlink_orb_listener.h" + +MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +{ + this->callback = callback; + this->arg = arg; + this->interval = interval * 1000; + this->mavlink = mavlink; + this->listener = listener; + this->subscriptions_n = subs_n; + this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); + + for (int i = 0; i < subs_n; i++) { + this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval); + } +} + +MavlinkStream::~MavlinkStream() +{ + free(subscriptions); +} + +/** + * Update mavlink stream, i.e. 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, update all subscriptions */ + for (unsigned int i = 0; i < subscriptions_n; i++) { + subscriptions[i]->update(t); + } + + /* format and send mavlink message */ + callback(this); + last_sent = t; + } +} diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h new file mode 100644 index 0000000000..0f959d7206 --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.h @@ -0,0 +1,33 @@ +/* + * mavlink_stream.h + * + * Created on: 24.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_STREAM_H_ +#define MAVLINK_STREAM_H_ + +class Mavlink; +class MavlinkOrbListener; +class MavlinkOrbSubscription; + +class MavlinkStream { +public: + void (*callback)(const MavlinkStream *); + uintptr_t arg; + unsigned int subscriptions_n; + MavlinkOrbSubscription **subscriptions; + hrt_abstime last_sent; + unsigned int interval; + MavlinkStream *next; + Mavlink *mavlink; + MavlinkOrbListener* listener; + + MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); + ~MavlinkStream(); + int update(const hrt_abstime t); +}; + + +#endif /* MAVLINK_STREAM_H_ */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 76798eb124..dc8d5586f4 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,8 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp + mavlink_orb_listener.cpp \ + mavlink_orb_subscription.cpp \ + mavlink_stream.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From cf7ac7e660b4be82d98b881fea6cbc718978605b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 25 Feb 2014 00:04:44 +0400 Subject: [PATCH 31/99] mavlink_orb_subscription: bug fixed --- src/modules/mavlink/mavlink_orb_subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 182407a5eb..16044ef72a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -41,7 +41,7 @@ int MavlinkOrbSubscription::update(const hrt_abstime t) bool updated; orb_check(fd, &updated); if (updated) - return orb_copy(meta, fd, &data); + return orb_copy(meta, fd, data); } return OK; } From 4e27fd9a381bd32ba5b79d275528ac19d1fb9442 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:22:19 +0400 Subject: [PATCH 32/99] commander/px4_custom_mode.h: added missing include --- src/modules/commander/px4_custom_mode.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9c..2144d34607 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 + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, From e291af990fd9a4f447cbad2416b78d031cd33f5c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:24:14 +0400 Subject: [PATCH 33/99] mavlink: adding message stream by name implemnted, mavlink streams definitions and formatters moved to mavlink_messages.h/cpp, mavlink_orb_listener class and thread removed --- src/modules/mavlink/mavlink_main.cpp | 442 ++++++------------ src/modules/mavlink/mavlink_main.h | 88 +--- ..._orb_listener.cpp => mavlink_messages.cpp} | 294 +++++------- src/modules/mavlink/mavlink_messages.h | 28 ++ src/modules/mavlink/mavlink_orb_listener.h | 138 ------ .../mavlink/mavlink_orb_subscription.cpp | 23 +- .../mavlink/mavlink_orb_subscription.h | 6 +- src/modules/mavlink/mavlink_stream.cpp | 8 +- src/modules/mavlink/mavlink_stream.h | 10 +- src/modules/mavlink/module.mk | 2 +- 10 files changed, 326 insertions(+), 713 deletions(-) rename src/modules/mavlink/{mavlink_orb_listener.cpp => mavlink_messages.cpp} (77%) create mode 100644 src/modules/mavlink/mavlink_messages.h delete mode 100644 src/modules/mavlink/mavlink_orb_listener.h diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ce467a7b8..4a75b00ab9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,11 +79,9 @@ #include #include -#include - #include "mavlink_bridge_header.h" #include "mavlink_main.h" -#include "mavlink_orb_listener.h" +#include "mavlink_messages.h" #include "mavlink_receiver.h" /* oddly, ERROR is not defined for c++ */ @@ -92,6 +90,8 @@ #endif static const int ERROR = -1; +#define MAIN_LOOP_DELAY 10000 // 100 Hz + static Mavlink* _head = nullptr; /* TODO: if this is a class member it crashes */ @@ -170,7 +170,8 @@ Mavlink::Mavlink() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _mavlink_hil_enabled(false), - // _params_sub(-1) + _subscriptions(nullptr), + _streams(nullptr), mission_pub(-1) { @@ -325,17 +326,6 @@ int Mavlink::get_channel() return (int)_chan; } -void -Mavlink::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - // param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); - -} - /**************************************************************************** * MAVLink text message logger ****************************************************************************/ @@ -532,13 +522,13 @@ Mavlink::set_hil_enabled(bool hil_enabled) 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); +// 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); +// orb_set_interval(subs.spa_sub, 200); } else { ret = ERROR; @@ -547,159 +537,8 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_base_mode = 0; - *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 (v_status.arming_state == ARMING_STATE_ARMED - || v_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 (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 Mavlink::set_mavlink_interval_limit(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(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; -} - extern mavlink_system_t mavlink_system; -void Mavlink::mavlink_pm_callback(void *arg, param_t param) -{ - //mavlink_pm_send_param(param); - usleep(*(unsigned int *)arg); -} - -void Mavlink::mavlink_pm_send_all_params(unsigned int delay) -{ - unsigned int dbuf = delay; - param_foreach(&mavlink_pm_callback, &dbuf, false); -} - int Mavlink::mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { @@ -1519,6 +1358,51 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +{ + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; + + LL_FOREACH(_subscriptions, sub) { + if (sub->topic == topic) { + /* already subscribed */ + return sub; + } + } + + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; +} + +int +Mavlink::add_stream(const char *stream_name, const unsigned int interval) +{ + uintptr_t arg = 0; + + unsigned int i = 0; + /* search for message with specified name */ + while (msgs_list[i].name != nullptr) { + if (strcmp(stream_name, msgs_list[i].name) == 0) { + /* count topics, array is nullptr-terminated */ + unsigned int topics_n; + for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) { + if (msgs_list[i].topics[topics_n] == nullptr) { + break; + } + } + MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval); + LL_APPEND(_streams, stream); + return OK; + } + i++; + } + return ERROR; +} + int Mavlink::task_main(int argc, char *argv[]) { @@ -1639,67 +1523,64 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ receive_thread = MavlinkReceiver::receive_start(this); - /* start the ORB receiver */ - uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); - /* 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_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 10 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); - /* 10 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - - } else if (_baudrate >= 115200) { - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - - } else if (_baudrate >= 57600) { - /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); - /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); - - } else { - /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); - /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); - /* 0.5 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); - /* 0.1 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); - } +// if (_baudrate >= 230400) { +// /* 200 Hz / 5 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); +// /* 50 Hz / 20 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); +// /* 20 Hz / 50 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); +// /* 10 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); +// /* 10 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); +// +// } else if (_baudrate >= 115200) { +// /* 20 Hz / 50 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); +// /* 2 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); +// +// } else if (_baudrate >= 57600) { +// /* 10 Hz / 100 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); +// /* 10 Hz / 100 ms ATTITUDE */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); +// /* 2 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); +// /* 2 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); +// +// } else { +// /* very low baud rate, limit to 1 Hz / 1000 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); +// /* 1 Hz / 1000 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); +// /* 0.5 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); +// /* 0.1 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); +// } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; @@ -1709,72 +1590,42 @@ Mavlink::task_main(int argc, char *argv[]) unsigned lowspeed_counter = 0; - /* wakeup source(s) */ - struct pollfd fds[1]; + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); + struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; + add_stream("HEARTBEAT", 1000); + add_stream("SYS_STATUS", 100); while (!_task_should_exit) { - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } + /* main loop */ + usleep(MAIN_LOOP_DELAY); perf_begin(_loop_perf); - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); + hrt_abstime t = hrt_absolute_time(); + + if (param_sub->update(t)) { + /* parameters updated */ + mavlink_update_system(); } - /* 1 Hz */ - if (lowspeed_counter % 10 == 0) { - 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); - + if (status_sub->update(t)) { /* switch HIL mode if required */ - if (v_status.hil_state == HIL_STATE_ON) + if (status->hil_state == HIL_STATE_ON) set_hil_enabled(true); - else if (v_status.hil_state == HIL_STATE_OFF) + else if (status->hil_state == HIL_STATE_OFF) set_hil_enabled(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 * 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); + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); } /* 0.5 Hz */ - if (lowspeed_counter % 20 == 0) { - + if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } @@ -1800,27 +1651,28 @@ Mavlink::task_main(int argc, char *argv[]) /* check if waypoint has been reached against the last positions */ mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* send parameters at 20 Hz (if queued for sending) */ - mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); - - mavlink_waypoint_eventloop(hrt_absolute_time()); - - if (_baudrate > 57600) { + if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { + /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(hrt_absolute_time()); + + mavlink_waypoint_eventloop(hrt_absolute_time()); + + if (_baudrate > 57600) { + mavlink_pm_queued_send(); + } + + /* 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); + } + } } - /* 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); - } - } - perf_end(_loop_perf); } @@ -1878,7 +1730,7 @@ int mavlink_main(int argc, char *argv[]) // Instantiate thread char buf[32]; - sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9461a51f8f..56d262000b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -41,6 +41,12 @@ #pragma once #include +#include +#include +#include +#include +#include +#include #include #include @@ -68,12 +74,12 @@ #include #include #include -#include #include #include -#include -#include +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -185,7 +191,7 @@ public: */ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + 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. @@ -197,57 +203,10 @@ public: */ int set_hil_enabled(bool hil_enabled); - 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 get_sub; - int airspeed_sub; - int navigation_capabilities_sub; - int position_setpoint_triplet_sub; - int rc_sub; - int status_sub; - int home_sub; - }; + MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - struct mavlink_subscriptions subs; - - struct mavlink_subscriptions* get_subs() { return &subs; } mavlink_channel_t get_chan() { return _chan; } - /** Global position */ - struct vehicle_global_position_s global_pos; - /** Local position */ - struct vehicle_local_position_s local_pos; - /** navigation capabilities */ - struct navigation_capabilities_s nav_cap; - /** Vehicle status */ - struct vehicle_status_s v_status; - /** RC channels */ - struct rc_channels_s rc; - /** Actuator armed state */ - struct actuator_armed_s armed; - /** Position setpoint triplet */ - struct position_setpoint_triplet_s pos_sp_triplet; - bool _task_should_exit; /**< if true, mavlink task should exit */ protected: @@ -270,7 +229,8 @@ private: /* states */ bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ - int _params_sub; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; orb_advert_t mission_pub; struct mission_s mission; @@ -306,21 +266,6 @@ private: bool mavlink_link_termination_allowed; - /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * 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. * @@ -381,12 +326,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval); - - /** - * Callback for param interface. - */ - static void mavlink_pm_callback(void *arg, param_t param); + int add_stream(const char *stream_name, const unsigned int interval); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_messages.cpp similarity index 77% rename from src/modules/mavlink/mavlink_orb_listener.cpp rename to src/modules/mavlink/mavlink_messages.cpp index ea1e3a8bb9..77ec344dc6 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,131 +1,138 @@ -/**************************************************************************** +/* + * mavlink_messages.cpp * - * 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 orb_listener.cpp - * Monitors ORB topics and sends update messages as appropriate. - * - * @author Lorenz Meier - * @author Julian Oes + * Created on: 25.02.2014 + * Author: ton */ -// XXX trim includes -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -#include +#include +#include -#include "mavlink_orb_listener.h" -#include "mavlink_main.h" +#include "mavlink_messages.h" -uint16_t cm_uint16_from_m_float(float m); +struct msgs_list_s msgs_list[] = { + { + .name = "HEARTBEAT", + .callback = msg_heartbeat, + .topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr }, + .sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) } + }, + { + .name = "SYS_STATUS", + .callback = msg_sys_status, + .topics = { ORB_ID(vehicle_status), nullptr }, + .sizes = { sizeof(struct vehicle_status_s) } + }, + { .name = nullptr } +}; -uint16_t -cm_uint16_from_m_float(float m) +void +msg_heartbeat(const MavlinkStream *stream) { - if (m < 0.0f) { - return 0; + struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; + struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data; - } else if (m > 655.35f) { - return 65535; + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - return (uint16_t)(m * 100.0f); -} + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } -MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), - _mavlink(parent), - _subscriptions(nullptr), - _streams(nullptr) -{ - -} - -MavlinkOrbListener::~MavlinkOrbListener() -{ - -} - -MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval) -{ - /* check if already subscribed to this topic */ - MavlinkOrbSubscription *sub; - - LL_FOREACH(_subscriptions, sub) { - if (sub->meta == meta) { - /* already subscribed */ - if (sub->interval > interval) { - /* subscribed with bigger interval, change interval */ - sub->set_interval(interval); - } - return sub; + /* 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; - /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(meta, size); + /* 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; + } - sub_new->set_interval(interval); - - LL_APPEND(_subscriptions, sub_new); - - return sub_new; + /* send heartbeat */ + mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); } -void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +void +msg_sys_status(const MavlinkStream *stream) { - MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval); + struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - stream->mavlink = _mavlink; - - LL_APPEND(_streams, stream); + mavlink_msg_sys_status_send(stream->mavlink->get_chan(), + 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); } //void @@ -259,25 +266,6 @@ void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), con // l->listener->gps_counter++; //} // - -void -MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream) -{ - /* 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; - //l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - // //void //MavlinkOrbListener::l_rc_channels(const struct listener *l) @@ -623,57 +611,3 @@ MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream) // l->listener->nav_cap.turn_distance); // //} - -void * -MavlinkOrbListener::uorb_receive_thread(void *arg) -{ - /* set thread name */ - char thread_name[18]; - sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); - prctl(PR_SET_NAME, thread_name, getpid()); - - /* add mavlink streams */ - /* common buffer for topics and data sizes */ - const struct orb_metadata *topics[1]; - size_t sizes[1]; - - /* --- HEARTBEAT --- */ - topics[0] = ORB_ID(vehicle_status); - sizes[0] = sizeof(vehicle_status_s); - add_stream(msg_heartbeat, 1, topics, sizes, 0, 500); - - while (!_mavlink->_task_should_exit) { - /* check all streams each 1ms */ - hrt_abstime t = hrt_absolute_time(); - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - stream->update(t); - } - usleep(1000); - } - - return NULL; -} - -void * MavlinkOrbListener::uorb_start_helper(void *context) -{ - MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context)); - return urcv->uorb_receive_thread(NULL); -} - -pthread_t -MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) -{ - /* 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, MavlinkOrbListener::uorb_start_helper, (void*)mavlink); - - pthread_attr_destroy(&uorb_attr); - return thread; -} diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h new file mode 100644 index 0000000000..78279c08f0 --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.h @@ -0,0 +1,28 @@ +/* + * mavlink_messages.h + * + * Created on: 25.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_MESSAGES_H_ +#define MAVLINK_MESSAGES_H_ + +#include "mavlink_stream.h" + +#define MAX_TOPICS_PER_MAVLINK_STREAM 4 + +struct msgs_list_s { + char *name; + void (*callback)(const MavlinkStream *); + const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1]; + size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1]; +}; + +extern struct msgs_list_s msgs_list[]; + +static void msg_heartbeat(const MavlinkStream *stream); +static void msg_sys_status(const MavlinkStream *stream); + + +#endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h deleted file mode 100644 index 1a103e43d3..0000000000 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * 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_orb_listener.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier - * @author Julian Oes - */ - -#include -#include - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mavlink_orb_subscription.h" -#include "mavlink_stream.h" - -class Mavlink; -class MavlinkStream; - -class MavlinkOrbListener -{ -public: - /** - * Constructor - */ - MavlinkOrbListener(Mavlink* parent); - - /** - * Destructor, closes all subscriptions. - */ - ~MavlinkOrbListener(); - - static pthread_t uorb_receive_start(Mavlink *mavlink); - - MavlinkOrbSubscription *add_subscription(const struct orb_metadata *meta, size_t size, const MavlinkStream *stream, const unsigned int interval); - void add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); - static void * uorb_start_helper(void *context); - -private: - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Mavlink* _mavlink; - - MavlinkOrbSubscription *_subscriptions; - static const unsigned _max_subscriptions = 32; - MavlinkStream *_streams; - - void *uorb_receive_thread(void *arg); - - static void msg_heartbeat(const MavlinkStream *stream); - -// 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); - -}; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 16044ef72a..84ac11483f 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,14 +13,13 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size) +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) { - this->meta = meta; + this->topic = topic; this->data = malloc(size); memset(this->data, 0, size); - this->fd = orb_subscribe(meta); + this->fd = orb_subscribe(topic); this->last_update = 0; - this->interval = 0; } MavlinkOrbSubscription::~MavlinkOrbSubscription() @@ -29,19 +28,15 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(data); } -int MavlinkOrbSubscription::set_interval(const unsigned int interval) -{ - this->interval = interval; - return orb_set_interval(fd, interval); -} - -int MavlinkOrbSubscription::update(const hrt_abstime t) +bool MavlinkOrbSubscription::update(const hrt_abstime t) { if (last_update != t) { bool updated; orb_check(fd, &updated); - if (updated) - return orb_copy(meta, fd, data); + if (updated) { + orb_copy(topic, fd, data); + return true; + } } - return OK; + return false; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 2c72abaeff..9a7340e9bb 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -16,14 +16,12 @@ public: MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); ~MavlinkOrbSubscription(); - int set_interval(const unsigned int interval); - int update(const hrt_abstime t); + bool update(const hrt_abstime t); - const struct orb_metadata *meta; + const struct orb_metadata *topic; int fd; void *data; hrt_abstime last_update; - unsigned int interval; MavlinkOrbSubscription *next; }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 2a6728fdb2..9df4263ee2 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -7,20 +7,20 @@ #include -#include "mavlink_orb_listener.h" +#include "mavlink_stream.h" +#include "mavlink_main.h" -MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval) { this->callback = callback; this->arg = arg; this->interval = interval * 1000; this->mavlink = mavlink; - this->listener = listener; this->subscriptions_n = subs_n; this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); for (int i = 0; i < subs_n; i++) { - this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval); + this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]); } } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 0f959d7206..c3e60917ef 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -8,8 +8,13 @@ #ifndef MAVLINK_STREAM_H_ #define MAVLINK_STREAM_H_ +#include + class Mavlink; -class MavlinkOrbListener; +class MavlinkStream; + +#include "mavlink_main.h" + class MavlinkOrbSubscription; class MavlinkStream { @@ -22,9 +27,8 @@ public: unsigned int interval; MavlinkStream *next; Mavlink *mavlink; - MavlinkOrbListener* listener; - MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); + MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval); ~MavlinkStream(); int update(const hrt_abstime t); }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index dc8d5586f4..5ea6c0f46a 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,8 +39,8 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp \ mavlink_orb_subscription.cpp \ + mavlink_messages.cpp \ mavlink_stream.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 769a2af1f8925a2d47fd47a2d25f8d7baac150ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:38:21 +0400 Subject: [PATCH 34/99] mavlink: HIGHRES_IMU message added, default message streams added --- src/modules/mavlink/mavlink_main.cpp | 72 +++++------------------- src/modules/mavlink/mavlink_messages.cpp | 17 ++++++ src/modules/mavlink/mavlink_messages.h | 2 +- 3 files changed, 33 insertions(+), 58 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4a75b00ab9..1563a257b9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1526,62 +1526,6 @@ Mavlink::task_main(int argc, char *argv[]) /* 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_MSG_ID_HIGHRES_IMU, 20); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); -// /* 50 Hz / 20 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); -// /* 20 Hz / 50 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); -// /* 10 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); -// /* 10 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); -// -// } else if (_baudrate >= 115200) { -// /* 20 Hz / 50 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); -// -// } else if (_baudrate >= 57600) { -// /* 10 Hz / 100 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); -// /* 10 Hz / 100 ms ATTITUDE */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); -// -// } else { -// /* very low baud rate, limit to 1 Hz / 1000 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); -// /* 1 Hz / 1000 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); -// /* 0.5 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); -// /* 0.1 Hz */ -// set_mavlink_interval_limit(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)); @@ -1595,8 +1539,22 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; + + /* add default streams, intervals depend on baud rate */ +// if (_baudrate >= 230400) { +// } else if (_baudrate >= 115200) { +// } else if (_baudrate >= 57600) { +// } + add_stream("HEARTBEAT", 1000); - add_stream("SYS_STATUS", 100); + add_stream("SYS_STATUS", 1000); + add_stream("HIGHRES_IMU", 300); + add_stream("RAW_IMU", 300); + add_stream("ATTITUDE", 200); + add_stream("NAMED_VALUE_FLOAT", 200); + add_stream("SERVO_OUTPUT_RAW", 500); + add_stream("GPS_RAW_INT", 500); + add_stream("MANUAL_CONTROL", 500); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 77ec344dc6..7a56c36a56 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -135,6 +135,23 @@ msg_sys_status(const MavlinkStream *stream) status->errors_count4); } +void +msg_highres_imu(const MavlinkStream *stream) +{ + struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data; + + uint16_t fields_updated = 0; + + if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), 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); +} + //void //MavlinkOrbListener::l_sensor_combined(const struct listener *l) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 78279c08f0..a6326bad1a 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -23,6 +23,6 @@ extern struct msgs_list_s msgs_list[]; static void msg_heartbeat(const MavlinkStream *stream); static void msg_sys_status(const MavlinkStream *stream); - +static void msg_highres_imu(const MavlinkStream *stream); #endif /* MAVLINK_MESSAGES_H_ */ From 7310fd608500be69153c5d033f74b056f1bb986e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 21:28:35 +0400 Subject: [PATCH 35/99] mavlink: use inherited classes instead of callbacks for mavlink messages formatting, fixes and cleanup --- src/modules/mavlink/mavlink_main.cpp | 202 ++++---- src/modules/mavlink/mavlink_main.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 440 +++++++++++------- src/modules/mavlink/mavlink_messages.h | 15 +- .../mavlink/mavlink_orb_subscription.cpp | 36 +- .../mavlink/mavlink_orb_subscription.h | 17 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/mavlink_stream.cpp | 49 +- src/modules/mavlink/mavlink_stream.h | 29 +- 9 files changed, 444 insertions(+), 362 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1563a257b9..33d81729c4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -152,12 +152,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -namespace mavlink -{ - - Mavlink *g_mavlink; -} - Mavlink::Mavlink() : device_name("/dev/ttyS1"), _task_should_exit(false), @@ -166,14 +160,13 @@ Mavlink::Mavlink() : thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _mavlink_hil_enabled(false), _subscriptions(nullptr), _streams(nullptr), + mission_pub(-1), - mission_pub(-1) +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; @@ -204,12 +197,14 @@ Mavlink::~Mavlink() } } -void Mavlink::set_mode(enum MAVLINK_MODE mode) +void +Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; } -int Mavlink::instance_count() +int +Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ Mavlink* inst = ::_head; @@ -222,10 +217,11 @@ int Mavlink::instance_count() return inst_index; } -Mavlink* Mavlink::new_instance() +Mavlink * +Mavlink::new_instance() { - Mavlink* inst = new Mavlink(); - Mavlink* next = ::_head; + Mavlink *inst = new Mavlink(); + Mavlink *next = ::_head; /* create the first instance at _head */ if (::_head == nullptr) { @@ -241,9 +237,10 @@ Mavlink* Mavlink::new_instance() return inst; } -Mavlink* Mavlink::get_instance(unsigned instance) +Mavlink * +Mavlink::get_instance(unsigned instance) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; while (inst->_next != nullptr && inst_index < instance) { inst = inst->_next; @@ -257,7 +254,8 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } -int Mavlink::destroy_all_instances() +int +Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; @@ -294,7 +292,8 @@ int Mavlink::destroy_all_instances() return OK; } -bool Mavlink::instance_exists(const char *device_name, Mavlink *self) +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; while (inst != nullptr) { @@ -307,7 +306,8 @@ bool Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } -int Mavlink::get_uart_fd(unsigned index) +int +Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); if (inst) @@ -316,14 +316,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } -int Mavlink::get_uart_fd() +int +Mavlink::get_uart_fd() { return _uart; } -int Mavlink::get_channel() +mavlink_channel_t +Mavlink::get_channel() { - return (int)_chan; + return _chan; } /**************************************************************************** @@ -1364,7 +1366,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->topic == topic) { + if (sub->get_topic() == topic) { /* already subscribed */ return sub; } @@ -1379,26 +1381,19 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata } int -Mavlink::add_stream(const char *stream_name, const unsigned int interval) +Mavlink::add_stream(const char *stream_name, const float rate) { - uintptr_t arg = 0; - - unsigned int i = 0; - /* search for message with specified name */ - while (msgs_list[i].name != nullptr) { - if (strcmp(stream_name, msgs_list[i].name) == 0) { - /* count topics, array is nullptr-terminated */ - unsigned int topics_n; - for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) { - if (msgs_list[i].topics[topics_n] == nullptr) { - break; - } - } - MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval); + /* search for stream with specified name */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create stream copy for each mavlink instance */ + MavlinkStream *stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(1000.0f / rate); + stream->subscribe(this); LL_APPEND(_streams, stream); return OK; } - i++; } return ERROR; } @@ -1407,7 +1402,7 @@ int Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ - warnx("Initializing.."); + warnx("start"); fflush(stdout); /* initialize mavlink text message buffering */ @@ -1463,9 +1458,6 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - /* inform about mode */ switch (_mode) { case MODE_TX_HEARTBEAT_ONLY: @@ -1510,13 +1502,13 @@ Mavlink::task_main(int argc, char *argv[]) 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); + if (instance_count() == 1) { + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + } /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* Initialize system properties */ mavlink_update_system(); @@ -1537,8 +1529,10 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; + 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, intervals depend on baud rate */ // if (_baudrate >= 230400) { @@ -1546,15 +1540,15 @@ Mavlink::task_main(int argc, char *argv[]) // } else if (_baudrate >= 57600) { // } - add_stream("HEARTBEAT", 1000); - add_stream("SYS_STATUS", 1000); - add_stream("HIGHRES_IMU", 300); - add_stream("RAW_IMU", 300); - add_stream("ATTITUDE", 200); - add_stream("NAMED_VALUE_FLOAT", 200); - add_stream("SERVO_OUTPUT_RAW", 500); - add_stream("GPS_RAW_INT", 500); - add_stream("MANUAL_CONTROL", 500); + add_stream("HEARTBEAT", 1.0f); + add_stream("SYS_STATUS", 1.0f); + add_stream("HIGHRES_IMU", 20.0f); +// add_stream("RAW_IMU", 10.0f); + add_stream("ATTITUDE", 20.0f); +// add_stream("NAMED_VALUE_FLOAT", 5.0f); +// add_stream("SERVO_OUTPUT_RAW", 2.0f); +// add_stream("GPS_RAW_INT", 2.0f); +// add_stream("MANUAL_CONTROL", 2.0f); while (!_task_should_exit) { /* main loop */ @@ -1604,31 +1598,24 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { mavlink_pm_queued_send(); } - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { + /* send one string at 20 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); } - } + } } perf_end(_loop_perf); @@ -1636,7 +1623,6 @@ Mavlink::task_main(int argc, char *argv[]) /* 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); @@ -1654,15 +1640,50 @@ Mavlink::task_main(int argc, char *argv[]) int Mavlink::start_helper(int argc, char *argv[]) { - // Create the instance in task context + /* create the instance in task context */ Mavlink *instance = Mavlink::new_instance(); - // This will actually only return once MAVLink exits + /* this will actually only return once MAVLink exits */ return instance->task_main(argc, argv); } int -Mavlink::start() +Mavlink::start(int argc, char *argv[]) { + // Instantiate thread + char buf[32]; + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } + + // if (mavlink::g_mavlink != nullptr) { + // errx(1, "already running"); + // } + + // mavlink::g_mavlink = new Mavlink; + + // if (mavlink::g_mavlink == nullptr) { + // errx(1, "alloc failed"); + // } + + // if (OK != mavlink::g_mavlink->start()) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + // err(1, "start failed"); + // } return OK; } @@ -1685,44 +1706,7 @@ int mavlink_main(int argc, char *argv[]) } if (!strcmp(argv[1], "start")) { - - // Instantiate thread - char buf[32]; - sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } - - return 0; + return Mavlink::start(argc, argv); } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 56d262000b..e7f3486da8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -142,7 +142,7 @@ public: * * @return OK on success. */ - static int start(); + static int start(int argc, char *argv[]); /** * Display the mavlink status. @@ -163,8 +163,6 @@ public: int get_uart_fd(); - int get_channel(); - const char *device_name; enum MAVLINK_MODE { @@ -205,16 +203,11 @@ public: MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - mavlink_channel_t get_chan() { return _chan; } + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - /** - * Pointer to the default cdev file operations table; useful for - * registering clone devices etc. - */ - Mavlink* _next; private: @@ -234,7 +227,7 @@ private: orb_advert_t mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; @@ -247,7 +240,6 @@ private: unsigned int total_counter; pthread_t receive_thread; - pthread_t uorb_receive_thread; /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; @@ -326,7 +318,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int add_stream(const char *stream_name, const unsigned int interval); + int add_stream(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7a56c36a56..df73581f05 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -12,195 +12,289 @@ #include "mavlink_messages.h" - -struct msgs_list_s msgs_list[] = { - { - .name = "HEARTBEAT", - .callback = msg_heartbeat, - .topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr }, - .sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) } - }, - { - .name = "SYS_STATUS", - .callback = msg_sys_status, - .topics = { ORB_ID(vehicle_status), nullptr }, - .sizes = { sizeof(struct vehicle_status_s) } - }, - { .name = nullptr } -}; - -void -msg_heartbeat(const MavlinkStream *stream) -{ - struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data; - - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - - /* HIL */ - if (status->hil_state == HIL_STATE_ON) { - mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; +class MavlinkStreamHeartbeat : public MavlinkStream { +public: + const char *get_name() + { + return "HEARTBEAT"; } - /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + MavlinkStream *new_instance() + { + return new MavlinkStreamHeartbeat(); } - /* 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) { +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; + + struct vehicle_status_s *status; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)status_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; + + /* 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; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + 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; + } } - } 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; + 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; + /* 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; + } + + 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"; } - /* send heartbeat */ - mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} + 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), sizeof(struct vehicle_status_s)); + 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: + 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 = 0; + uint32_t gyro_counter = 0; + uint32_t mag_counter = 0; + uint32_t baro_counter = 0; + +protected: + + void subscribe(Mavlink *mavlink) + { + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + 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), sizeof(struct vehicle_attitude_s)); + 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); + } +}; + + +MavlinkStream *streams_list[] = { + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + nullptr +}; + + + -void -msg_sys_status(const MavlinkStream *stream) -{ - struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - mavlink_msg_sys_status_send(stream->mavlink->get_chan(), - 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); -} -void -msg_highres_imu(const MavlinkStream *stream) -{ - struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data; - uint16_t fields_updated = 0; - if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), 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); -} -//void -//MavlinkOrbListener::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), l->mavlink->get_subs()->sensor_sub, &raw); -// -// /* mark individual fields as _mavlink->get_chan()ged */ -// uint16_t fields_updated = 0; -// -// // if (accel_counter != raw.accelerometer_counter) { -// // /* mark first three dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); -// // accel_counter = raw.accelerometer_counter; -// // } -// -// // if (gyro_counter != raw.gyro_counter) { -// // /* mark second group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); -// // gyro_counter = raw.gyro_counter; -// // } -// -// // if (mag_counter != raw.magnetometer_counter) { -// // /* mark third group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); -// // mag_counter = raw.magnetometer_counter; -// // } -// -// // if (baro_counter != raw.baro_counter) { -// // /* mark last group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); -// // baro_counter = raw.baro_counter; -// // } -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->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); -// -// l->listener->sensors_raw_counter++; -//} -// //void //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index a6326bad1a..3dc6cb6998 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -10,19 +10,6 @@ #include "mavlink_stream.h" -#define MAX_TOPICS_PER_MAVLINK_STREAM 4 - -struct msgs_list_s { - char *name; - void (*callback)(const MavlinkStream *); - const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1]; - size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1]; -}; - -extern struct msgs_list_s msgs_list[]; - -static void msg_heartbeat(const MavlinkStream *stream); -static void msg_sys_status(const MavlinkStream *stream); -static void msg_highres_imu(const MavlinkStream *stream); +extern MavlinkStream *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 84ac11483f..b504b6955b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,28 +13,40 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) { - this->topic = topic; - this->data = malloc(size); - memset(this->data, 0, size); - this->fd = orb_subscribe(topic); - this->last_update = 0; + _data = malloc(size); + memset(_data, 0, size); + _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() { - close(fd); - free(data); + close(_fd); + free(_data); } -bool MavlinkOrbSubscription::update(const hrt_abstime t) +const struct orb_metadata * +MavlinkOrbSubscription::get_topic() { - if (last_update != t) { + return _topic; +} + +void * +MavlinkOrbSubscription::get_data() +{ + return _data; +} + +bool +MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (_last_check != t) { + _last_check = t; bool updated; - orb_check(fd, &updated); + orb_check(_fd, &updated); if (updated) { - orb_copy(topic, fd, data); + orb_copy(_topic, _fd, _data); return true; } } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 9a7340e9bb..c38a9cc431 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -11,18 +11,23 @@ #include #include + class MavlinkOrbSubscription { public: - MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); + MavlinkOrbSubscription *next; + + MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + void *get_data(); + const struct orb_metadata *get_topic(); - const struct orb_metadata *topic; - int fd; - void *data; - hrt_abstime last_update; - MavlinkOrbSubscription *next; +private: + const struct orb_metadata *_topic; + int _fd; + void *_data; + hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d07de0f22f..b828420e68 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -818,7 +818,7 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 9df4263ee2..16407366e4 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -10,39 +10,42 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) { - this->callback = callback; - this->arg = arg; - this->interval = interval * 1000; - this->mavlink = mavlink; - this->subscriptions_n = subs_n; - this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); - - for (int i = 0; i < subs_n; i++) { - this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]); - } } MavlinkStream::~MavlinkStream() { - free(subscriptions); } /** - * Update mavlink stream, i.e. update subscriptions and send message if necessary + * Set messages interval in ms */ -int MavlinkStream::update(const hrt_abstime t) +void +MavlinkStream::set_interval(const unsigned int interval) { - uint64_t dt = t - last_sent; - if (dt > 0 && dt >= interval) { - /* interval expired, update all subscriptions */ - for (unsigned int i = 0; i < subscriptions_n; i++) { - subscriptions[i]->update(t); - } + _interval = interval * 1000; +} - /* format and send mavlink message */ - callback(this); - last_sent = t; +/** + * 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 index c3e60917ef..9f175adbe5 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -15,22 +15,27 @@ class MavlinkStream; #include "mavlink_main.h" -class MavlinkOrbSubscription; - class MavlinkStream { -public: - void (*callback)(const MavlinkStream *); - uintptr_t arg; - unsigned int subscriptions_n; - MavlinkOrbSubscription **subscriptions; - hrt_abstime last_sent; - unsigned int interval; - MavlinkStream *next; - Mavlink *mavlink; +private: + hrt_abstime _last_sent; - MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval); +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; }; From 2967763b160bb12b8e13e72219fcb689da015ab5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:13:49 +0400 Subject: [PATCH 36/99] mavlink: heartbeat message bug fixed --- src/modules/mavlink/mavlink_messages.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index df73581f05..0880e8285b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -38,7 +38,7 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { @@ -62,6 +62,7 @@ protected: /* 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) { @@ -96,7 +97,6 @@ protected: 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 @@ -119,7 +119,7 @@ protected: mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, - mavlink_custom_mode, + custom_mode.data, mavlink_state); } From f9619e39341dbc45683c6ccd7e06397f8a537216 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:46:33 +0400 Subject: [PATCH 37/99] geo: _wrap_XXX minor fix --- src/lib/geo/geo.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9b3e202e6e..59c04f0e39 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; From 85dc422d9804c894009e37c6eaab67a10c5dca28 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:47:19 +0400 Subject: [PATCH 38/99] mavlink: more streams implemented, stack size returned to 2048 --- src/modules/mavlink/mavlink_main.cpp | 26 ++- src/modules/mavlink/mavlink_messages.cpp | 204 +++++++++++++++++------ 2 files changed, 164 insertions(+), 66 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 33d81729c4..5ac76e1cf0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -61,14 +61,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -79,6 +71,10 @@ #include #include +#include +#include +#include + #include "mavlink_bridge_header.h" #include "mavlink_main.h" #include "mavlink_messages.h" @@ -1542,13 +1538,11 @@ Mavlink::task_main(int argc, char *argv[]) add_stream("HEARTBEAT", 1.0f); add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 20.0f); -// add_stream("RAW_IMU", 10.0f); - add_stream("ATTITUDE", 20.0f); -// add_stream("NAMED_VALUE_FLOAT", 5.0f); -// add_stream("SERVO_OUTPUT_RAW", 2.0f); -// add_stream("GPS_RAW_INT", 2.0f); -// add_stream("MANUAL_CONTROL", 2.0f); + add_stream("HIGHRES_IMU", 1.0f); + add_stream("ATTITUDE", 10.0f); + add_stream("GPS_RAW_INT", 1.0f); + add_stream("GLOBAL_POSITION_INT", 5.0f); + add_stream("LOCAL_POSITION_NED", 5.0f); while (!_task_should_exit) { /* main loop */ @@ -1656,7 +1650,7 @@ Mavlink::start(int argc, char *argv[]) /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 4096, + 2048, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0880e8285b..27ae197a65 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -6,12 +6,32 @@ */ #include +#include +#include +#include +#include #include #include +#include +#include #include "mavlink_messages.h" + +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); +} + class MavlinkStreamHeartbeat : public MavlinkStream { public: const char *get_name() @@ -279,11 +299,145 @@ protected: }; +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), sizeof(struct vehicle_gps_position_s)); + 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; + MavlinkOrbSubscription *home_sub; + + struct vehicle_global_position_s *pos; + struct home_position_s *home; + +protected: + + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + 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), sizeof(struct vehicle_local_position_s)); + 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); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamGPSRawInt(), + new MavlinkStreamGlobalPositionInt(), + new MavlinkStreamLocalPositionNED(), nullptr }; @@ -347,22 +501,6 @@ MavlinkStream *streams_list[] = { // /* copy gps data into local buffer */ // orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_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(l->mavlink->get_chan(), -// 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 && (l->listener->gps_counter % 50 == 0)) { // mavlink_msg_gps_status_send(l->mavlink->get_chan(), @@ -414,40 +552,6 @@ MavlinkStream *streams_list[] = { // } //} // -//void -//MavlinkOrbListener::l_global_position(const struct listener *l) -//{ -// /* copy global position data into local buffer */ -// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); -// -// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), -// l->listener->global_pos.timestamp / 1000, -// l->listener->global_pos.lat * 1e7, -// l->listener->global_pos.lon * 1e7, -// l->listener->global_pos.alt * 1000.0f, -// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, -// l->listener->global_pos.vel_n * 100.0f, -// l->listener->global_pos.vel_e * 100.0f, -// l->listener->global_pos.vel_d * 100.0f, -// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -//} -// -//void -//MavlinkOrbListener::l_local_position(const struct listener *l) -//{ -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), -// l->listener->local_pos.timestamp / 1000, -// l->listener->local_pos.x, -// l->listener->local_pos.y, -// l->listener->local_pos.z, -// l->listener->local_pos.vx, -// l->listener->local_pos.vy, -// l->listener->local_pos.vz); -//} // //void //MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) From 18f72f8dd7a3bd4a9af00fe28afd29e0cc65e2e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 23:02:53 +0400 Subject: [PATCH 39/99] mavlink: GPS_GLOBAL_ORIGIN message added, set message rate depending on baudrate --- src/modules/mavlink/mavlink_main.cpp | 21 +++++----- src/modules/mavlink/mavlink_messages.cpp | 53 +++++++++++++++++------- 2 files changed, 48 insertions(+), 26 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5ac76e1cf0..3102c937da 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1531,18 +1531,19 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* add default streams, intervals depend on baud rate */ -// if (_baudrate >= 230400) { -// } else if (_baudrate >= 115200) { -// } else if (_baudrate >= 57600) { -// } + float rate_mult = _baudrate / 57600.0f; + if (rate_mult > 4.0f) { + rate_mult = 4.0f; + } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 1.0f); - add_stream("ATTITUDE", 10.0f); - add_stream("GPS_RAW_INT", 1.0f); - add_stream("GLOBAL_POSITION_INT", 5.0f); - add_stream("LOCAL_POSITION_NED", 5.0f); + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 27ae197a65..48443fbdc0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -430,6 +430,42 @@ protected: }; +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), sizeof(struct home_position_s)); + 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); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -438,6 +474,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), nullptr }; @@ -517,14 +554,6 @@ MavlinkStream *streams_list[] = { // // //void -//MavlinkOrbListener::l_rc_channels(const struct listener *l) -//{ -// /* copy rc channels into local buffer */ -// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); -// // XXX Add RC channels scaled message here -//} -// -//void //MavlinkOrbListener::l_input_rc(const struct listener *l) //{ // /* copy rc _mavlink->get_chan()nels into local buffer */ @@ -801,14 +830,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_home(const struct listener *l) -//{ -// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); -// -// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -//} -// -//void //MavlinkOrbListener::l_airspeed(const struct listener *l) //{ // orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); From 141982a3ac21e7a0437f1d7692e4024daf873c21 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 13:54:55 +0400 Subject: [PATCH 40/99] mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added --- src/modules/mavlink/mavlink_main.cpp | 68 +-- src/modules/mavlink/mavlink_main.h | 9 +- src/modules/mavlink/mavlink_messages.cpp | 438 ++++++++++-------- .../mavlink/mavlink_orb_subscription.cpp | 2 +- .../mavlink/mavlink_orb_subscription.h | 4 +- src/modules/mavlink/mavlink_rate_limiter.cpp | 38 ++ src/modules/mavlink/mavlink_rate_limiter.h | 28 ++ src/modules/mavlink/mavlink_stream.cpp | 2 +- src/modules/mavlink/module.mk | 3 +- 9 files changed, 371 insertions(+), 221 deletions(-) create mode 100644 src/modules/mavlink/mavlink_rate_limiter.cpp create mode 100644 src/modules/mavlink/mavlink_rate_limiter.h diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3102c937da..8a026742c3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,6 +79,7 @@ #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 @@ -321,7 +322,7 @@ Mavlink::get_uart_fd() mavlink_channel_t Mavlink::get_channel() { - return _chan; + return _channel; } /**************************************************************************** @@ -1321,7 +1322,7 @@ 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(_chan, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); } @@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; - _chan = MAVLINK_COMM_0; + _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - unsigned lowspeed_counter = 0; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); @@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[]) warnx("started"); mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams, intervals depend on baud rate */ + /* add default streams depending on mode, intervals depend on baud rate */ float rate_mult = _baudrate / 57600.0f; if (rate_mult > 4.0f) { rate_mult = 4.0f; } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f * rate_mult); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + + switch(_mode) { + case MODE_OFFBOARD: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + break; + + case MODE_HIL: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + break; + + default: + break; + } + + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); while (!_task_should_exit) { /* main loop */ @@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - /* 0.5 Hz */ - if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - } - - lowspeed_counter++; - bool updated; orb_check(mission_result_sub, &updated); @@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[]) } 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 (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { - /* send parameters at 20 Hz (if queued for sending) */ + if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { - mavlink_pm_queued_send(); - } - - /* send one string at 20 Hz */ if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e7f3486da8..506b4317ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -201,7 +201,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size); mavlink_channel_t get_channel(); @@ -231,10 +231,7 @@ private: MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _chan; - -// XXX probably should be in a header... -// extern pthread_t receive_start(int uart); + mavlink_channel_t _channel; struct mavlink_logbuffer lb; unsigned int total_counter; @@ -248,7 +245,7 @@ private: bool _verbose; int _uart; int _baudrate; - bool gcs_link; + /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 48443fbdc0..9676068419 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -5,6 +5,7 @@ * Author: ton */ +#include #include #include @@ -19,6 +20,10 @@ #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) { @@ -32,6 +37,83 @@ cm_uint16_from_m_float(float m) 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() @@ -68,78 +150,13 @@ protected: uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t 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; - } - } - - /* 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; - } + 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, - custom_mode.data, + mavlink_custom_mode, mavlink_state); } @@ -164,7 +181,6 @@ private: struct vehicle_status_s *status; protected: - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); @@ -215,7 +231,6 @@ private: uint32_t baro_counter = 0; protected: - void subscribe(Mavlink *mavlink) { sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); @@ -281,7 +296,6 @@ private: struct vehicle_attitude_s *att; protected: - void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); @@ -313,11 +327,9 @@ public: 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), sizeof(struct vehicle_gps_position_s)); @@ -356,13 +368,12 @@ public: private: MavlinkOrbSubscription *pos_sub; - MavlinkOrbSubscription *home_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), sizeof(struct vehicle_global_position_s)); @@ -404,11 +415,9 @@ public: 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), sizeof(struct vehicle_local_position_s)); @@ -444,11 +453,9 @@ public: private: MavlinkOrbSubscription *home_sub; - struct home_position_s *home; protected: - void subscribe(Mavlink *mavlink) { home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); @@ -466,6 +473,171 @@ protected: }; +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], sizeof(struct actuator_outputs_s)); + 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), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + 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), sizeof(struct actuator_outputs_s)); + 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); + + /* 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(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->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(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->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(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + ((act->output[6] - 900.0f) / 600.0f) / 2.0f, + ((act->output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); + + } else { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + (act->output[0] - 1500.0f) / 500.0f, + (act->output[1] - 1500.0f) / 500.0f, + (act->output[2] - 1500.0f) / 500.0f, + (act->output[3] - 1000.0f) / 1000.0f, + (act->output[4] - 1500.0f) / 500.0f, + (act->output[5] - 1500.0f) / 500.0f, + (act->output[6] - 1500.0f) / 500.0f, + (act->output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -475,6 +647,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), new MavlinkStreamGPSGlobalOrigin(), + new MavlinkStreamServoOutputRaw(0), + new MavlinkStreamServoOutputRaw(1), + new MavlinkStreamServoOutputRaw(2), + new MavlinkStreamServoOutputRaw(3), + new MavlinkStreamHILControls(), nullptr }; @@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = { - - //void //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) //{ @@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::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 (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->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 (l->mavlink->get_hil_enabled() && l->listener->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; -// l->mavlink->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(l->mavlink->get_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(l->mavlink->get_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(l->mavlink->get_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(l->mavlink->get_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 //MavlinkOrbListener::l_actuator_armed(const struct listener *l) //{ // orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index b504b6955b..35470a19a1 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,7 +13,7 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) { _data = malloc(size); memset(_data, 0, size); diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index c38a9cc431..79ff3abdbe 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -16,7 +16,7 @@ class MavlinkOrbSubscription { public: MavlinkOrbSubscription *next; - MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); + MavlinkOrbSubscription(const orb_id_t topic, size_t size); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); @@ -24,7 +24,7 @@ public: const struct orb_metadata *get_topic(); private: - const struct orb_metadata *_topic; + const orb_id_t _topic; int _fd; void *_data; hrt_abstime _last_check; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp new file mode 100644 index 0000000000..f5bb06ccd2 --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -0,0 +1,38 @@ +/* + * mavlink_rate_limiter.cpp + * + * Created on: 26.02.2014 + * Author: ton + */ + + +#include "mavlink_rate_limiter.h" + +MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000) +{ +} + +MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000) +{ +} + +MavlinkRateLimiter::~MavlinkRateLimiter() +{ +} + +void +MavlinkRateLimiter::set_interval(unsigned int interval) +{ + _interval = interval * 1000; +} + +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/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h new file mode 100644 index 0000000000..6db65f638f --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -0,0 +1,28 @@ +/* + * mavlink_rate_limiter.h + * + * Created on: 26.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_RATE_LIMITER_H_ +#define MAVLINK_RATE_LIMITER_H_ + +#include + + +class MavlinkRateLimiter { +private: + hrt_abstime _last_sent; + hrt_abstime _interval; + +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_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 16407366e4..703f74b4c3 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -10,7 +10,7 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) { } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 5ea6c0f46a..f09efa2e6c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -41,6 +41,7 @@ SRCS += mavlink_main.cpp \ mavlink_receiver.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ - mavlink_stream.cpp + mavlink_stream.cpp \ + mavlink_rate_limiter.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 2159f948ea81088076b440cd8b673b1ac9f70da3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:31:41 +0400 Subject: [PATCH 41/99] mavlink: -r (datarate) parameter implemented, minor fixes --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- ROMFS/px4fmu_common/init.d/rcS | 4 +- src/modules/mavlink/mavlink_main.cpp | 83 +++++++++++++++++------- src/modules/mavlink/mavlink_main.h | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- 5 files changed, 65 insertions(+), 30 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 3d8be089ee..558be42755 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -b 230400 -d /dev/ttyACM0 +mavlink start -r 10000 -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 76f021e339..c3065b6fc8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -390,14 +390,14 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - mavlink start -d /dev/ttyS0 + mavlink start -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 - mavlink start + mavlink start -r 1000 usleep 5000 fi fi diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8a026742c3..974fd27bf9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -87,6 +87,7 @@ #endif static const int ERROR = -1; +#define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz static Mavlink* _head = nullptr; @@ -449,7 +450,6 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* 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 */ @@ -1407,6 +1407,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; + _datarate = 0; _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1415,7 +1416,7 @@ Mavlink::task_main(int argc, char *argv[]) argc -= 2; argv += 2; - while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1425,6 +1426,14 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'r': + _datarate = strtoul(optarg, NULL, 10); + + if (_datarate < 10 || _datarate > MAX_DATA_RATE) + errx(1, "invalid data rate '%s'", optarg); + + break; + case 'd': device_name = optarg; break; @@ -1433,8 +1442,19 @@ Mavlink::task_main(int argc, char *argv[]) // mavlink_link_termination_allowed = true; // break; - case 'o': - _mode = MODE_ONBOARD; + 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': @@ -1447,51 +1467,61 @@ Mavlink::task_main(int argc, char *argv[]) } } + 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); } - struct termios uart_config_original; - - bool usb_uart; - /* inform about mode */ switch (_mode) { - case MODE_TX_HEARTBEAT_ONLY: - warnx("MODE_TX_HEARTBEAT_ONLY"); + case MODE_CUSTOM: + warnx("mode: CUSTOM"); break; case MODE_OFFBOARD: - warnx("MODE_OFFBOARD"); + warnx("mode: OFFBOARD"); break; case MODE_ONBOARD: - warnx("MODE_ONBOARD"); + warnx("mode: ONBOARD"); break; case MODE_HIL: - warnx("MODE_HIL"); + warnx("mode: HIL"); break; default: - warnx("Error: Unknown mode"); + 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; - case MODE_TX_HEARTBEAT_ONLY: default: _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; - warnx("Error: Unknown mode"); break; } - /* Flush stdout in case MAVLink is about to take it over */ + 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 = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); @@ -1529,17 +1559,14 @@ Mavlink::task_main(int argc, char *argv[]) warnx("started"); mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams depending on mode, intervals depend on baud rate */ - float rate_mult = _baudrate / 57600.0f; - if (rate_mult > 4.0f) { - rate_mult = 4.0f; - } + /* add default streams depending on mode and intervals depending on datarate */ + float rate_mult = _datarate / 1000.0f; add_stream("HEARTBEAT", 1.0f); switch(_mode) { case MODE_OFFBOARD: - add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("SYS_STATUS", 1.0f); add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); add_stream("HIGHRES_IMU", 1.0f * rate_mult); add_stream("ATTITUDE", 10.0f * rate_mult); @@ -1550,13 +1577,14 @@ Mavlink::task_main(int argc, char *argv[]) break; case MODE_HIL: - add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("SYS_STATUS", 1.0f); add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); add_stream("HIGHRES_IMU", 1.0f * rate_mult); add_stream("ATTITUDE", 10.0f * rate_mult); add_stream("GPS_RAW_INT", 1.0f * rate_mult); add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + add_stream("HIL_CONTROLS", 20.0f * rate_mult); break; default: @@ -1703,7 +1731,12 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-r datarate] [-m mode] [-v]\n\n" + "Supported modes (-m):\n" + "\toffboard\tSend standard telemetry data to ground station (default)\n" + "\tonboard\tOnboard comminication mode, e.g. to connect PX4FLOW\n" + "\thil\tHardware In the Loop mode, send telemetry and HIL_CONTROLS\n" + "\tcustom\tCustom configuration, don't send anything by default, streams can be enabled by 'mavlink stream' command\n"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 506b4317ad..afbf857877 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -166,7 +166,7 @@ public: const char *device_name; enum MAVLINK_MODE { - MODE_TX_HEARTBEAT_ONLY=0, + MODE_CUSTOM=0, MODE_OFFBOARD, MODE_ONBOARD, MODE_HIL @@ -245,6 +245,7 @@ private: bool _verbose; int _uart; int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b828420e68..a3546e954b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -250,7 +250,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (mavlink_system.sysid < 4) { /* switch to a receiving link mode */ - _mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); + //TODO why do we need this? + //_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); /* * rate control mode - defined by MAVLink From efca2d158aa34c912be91fb3229b211022ae0945 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 00:45:59 +0400 Subject: [PATCH 42/99] mavlink: commanl line streams configuration implemented --- src/modules/mavlink/mavlink_main.cpp | 221 ++++++++++++++++++++------- src/modules/mavlink/mavlink_main.h | 12 +- 2 files changed, 175 insertions(+), 58 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 974fd27bf9..672daf641b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -87,6 +87,7 @@ #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 @@ -151,9 +152,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - device_name("/dev/ttyS1"), + device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), - _next(nullptr), + next(nullptr), _mavlink_fd(-1), thread_running(false), _mavlink_task(-1), @@ -208,7 +209,7 @@ Mavlink::instance_count() Mavlink* inst = ::_head; unsigned inst_index = 0; while (inst != nullptr) { - inst = inst->_next; + inst = inst->next; inst_index++; } @@ -226,11 +227,11 @@ Mavlink::new_instance() ::_head = inst; /* afterwards follow the next and append the instance */ } else { - while (next->_next != nullptr) { - next = next->_next; + while (next->next != nullptr) { + next = next->next; } /* now parent has a null pointer, fill it */ - next->_next = inst; + next->next = inst; } return inst; } @@ -240,8 +241,8 @@ Mavlink::get_instance(unsigned instance) { Mavlink *inst = ::_head; unsigned inst_index = 0; - while (inst->_next != nullptr && inst_index < instance) { - inst = inst->_next; + while (inst->next != nullptr && inst_index < instance) { + inst = inst->next; inst_index++; } @@ -252,6 +253,20 @@ Mavlink::get_instance(unsigned instance) return inst; } +Mavlink * +Mavlink::get_instance_for_device(const char *device_name) +{ + Mavlink *inst; + + LL_FOREACH(::_head, inst) { + if (strcmp(inst->device_name, device_name) == 0) { + return inst; + } + } + + return nullptr; +} + int Mavlink::destroy_all_instances() { @@ -265,7 +280,7 @@ Mavlink::destroy_all_instances() while (next_inst != nullptr) { inst_to_del = next_inst; - next_inst = inst_to_del->_next; + 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; @@ -299,7 +314,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) /* don't compare with itself */ if (inst != self && !strcmp(device_name, inst->device_name)) return true; - inst = inst->_next; + inst = inst->next; } return false; } @@ -348,7 +363,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) mavlink_logbuffer_write(&inst->lb, &msg); inst->total_counter++; - inst = inst->_next; + inst = inst->next; } return OK; @@ -1378,20 +1393,47 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, cons } int -Mavlink::add_stream(const char *stream_name, const float rate) +Mavlink::configure_stream(const char *stream_name, const float rate) { - /* search for stream with specified name */ - for (unsigned int i = 0; streams_list[i] != nullptr; i++) { - if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { - /* create stream copy for each mavlink instance */ - MavlinkStream *stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); - stream->set_interval(1000.0f / rate); - stream->subscribe(this); - LL_APPEND(_streams, stream); + /* calculate interval in ms, 0 means disabled stream */ + unsigned int interval = (rate > 0.0f) ? (1000.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; } @@ -1416,22 +1458,26 @@ Mavlink::task_main(int argc, char *argv[]) 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) - errx(1, "invalid baud rate '%s'", optarg); - + 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) - errx(1, "invalid data rate '%s'", optarg); - + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { + warnx("invalid data rate '%s'", optarg); + err_flag = true; + } break; case 'd': @@ -1462,11 +1508,16 @@ Mavlink::task_main(int argc, char *argv[]) break; default: - usage(); + 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; @@ -1562,29 +1613,29 @@ Mavlink::task_main(int argc, char *argv[]) /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; - add_stream("HEARTBEAT", 1.0f); + configure_stream("HEARTBEAT", 1.0f); switch(_mode) { case MODE_OFFBOARD: - add_stream("SYS_STATUS", 1.0f); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f * rate_mult); + configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("GPS_RAW_INT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); break; case MODE_HIL: - add_stream("SYS_STATUS", 1.0f); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - add_stream("HIL_CONTROLS", 20.0f * rate_mult); + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f * rate_mult); + configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("GPS_RAW_INT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + configure_stream("HIL_CONTROLS", 20.0f * rate_mult); break; default: @@ -1726,17 +1777,76 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::status() { - warnx("Running"); + 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) { + if (OK == inst->configure_stream(stream_name, rate)) { + if (rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", stream_name, device_name, rate); + + } else { + warnx("stream %s on device %s disabled", stream_name, device_name); + } + + } else { + warnx("stream %s not found", stream_name, device_name); + } + + } 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} [-d device] [-b baudrate] [-r datarate] [-m mode] [-v]\n\n" - "Supported modes (-m):\n" - "\toffboard\tSend standard telemetry data to ground station (default)\n" - "\tonboard\tOnboard comminication mode, e.g. to connect PX4FLOW\n" - "\thil\tHardware In the Loop mode, send telemetry and HIL_CONTROLS\n" - "\tcustom\tCustom configuration, don't send anything by default, streams can be enabled by 'mavlink stream' command\n"); + 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[]) @@ -1758,6 +1868,9 @@ int mavlink_main(int argc, char *argv[]) // } else if (!strcmp(argv[1], "status")) { // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "stream")) { + return Mavlink::stream(argc, argv); + } else { usage(); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index afbf857877..049f5fedd5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -149,11 +149,15 @@ public: */ void status(); + static int stream(int argc, char *argv[]); + static int instance_count(); - static Mavlink* new_instance(); + static Mavlink *new_instance(); - static Mavlink* get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); static int destroy_all_instances(); @@ -208,7 +212,7 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - Mavlink* _next; + Mavlink* next; private: int _mavlink_fd; @@ -316,7 +320,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int add_stream(const char *stream_name, const float rate); + int configure_stream(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); From 967f81bfabbab0773fa29c079e36678670bcf67a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 23:43:52 +0400 Subject: [PATCH 43/99] mavlink_onboard removed --- makefiles/config_px4fmu-v2_default.mk | 1 - src/modules/mavlink_onboard/mavlink.c | 537 ------------------ .../mavlink_onboard/mavlink_bridge_header.h | 83 --- .../mavlink_onboard/mavlink_receiver.c | 344 ----------- src/modules/mavlink_onboard/module.mk | 42 -- src/modules/mavlink_onboard/orb_topics.h | 102 ---- src/modules/mavlink_onboard/util.h | 55 -- 7 files changed, 1164 deletions(-) delete mode 100644 src/modules/mavlink_onboard/mavlink.c delete mode 100644 src/modules/mavlink_onboard/mavlink_bridge_header.h delete mode 100644 src/modules/mavlink_onboard/mavlink_receiver.c delete mode 100644 src/modules/mavlink_onboard/module.mk delete mode 100644 src/modules/mavlink_onboard/orb_topics.h delete mode 100644 src/modules/mavlink_onboard/util.h diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a95..412d0c9e18 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/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c deleted file mode 100644 index ab9ce45f3e..0000000000 --- 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 - * - * 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 - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#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 ] [-b ]\n" - " mavlink_onboard stop\n" - " mavlink_onboard status\n"); - exit(1); -} - -int mavlink_onboard_main(int argc, char *argv[]) -{ - - if (argc < 2) { - warnx("missing command"); - usage(); - } - - if (!strcmp(argv[1], "start")) { - - /* this is not an error */ - if (thread_running) - errx(0, "already running"); - - thread_should_exit = false; - mavlink_task = task_spawn_cmd("mavlink_onboard", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - mavlink_thread_main, - (const char**)argv); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - while (thread_running) { - usleep(200000); - } - warnx("terminated"); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -} - diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h deleted file mode 100644 index b72bbb2b19..0000000000 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_bridge_header - * MAVLink bridge header for UART access. - * - * @author Lorenz Meier - */ - -/* 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 -#include - - -/* Struct that stores the communication settings of this system. - you can also define / alter these settings elsewhere, as long - as they're included BEFORE mavlink.h. - So you can set the - - mavlink_system.sysid = 100; // System ID, 1-255 - mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 - - Lines also in your main.c, e.g. by reading these parameter from EEPROM. - */ -extern mavlink_system_t mavlink_system; - -/** - * @brief Send multiple chars (uint8_t) over a comm channel - * - * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 - * @param ch Character to send - */ -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); - -mavlink_status_t* mavlink_get_channel_status(uint8_t chan); -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); - -#include - -#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c deleted file mode 100644 index 4658bcc1d4..0000000000 --- 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 - * - * 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 - */ - -/* XXX trim includes */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#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 a7a4980faf..0000000000 --- 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 bbc9f6e66a..0000000000 --- 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 - * - * 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int safety_sub; - int actuators_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int spg_sub; - int debug_key_value; - int input_rc_sub; -}; - -extern struct mavlink_subscriptions mavlink_subs; - -/** Global position */ -extern struct vehicle_global_position_s global_pos; - -/** Local position */ -extern struct vehicle_local_position_s local_pos; - -/** Vehicle status */ -// extern struct vehicle_status_s v_status; - -/** RC channels */ -extern struct rc_channels_s rc; - -/** Actuator armed state */ -// extern struct actuator_armed_s armed; - -/** Worker thread starter */ -extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h deleted file mode 100644 index c84b6fd26e..0000000000 --- a/src/modules/mavlink_onboard/util.h +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file util.h - * Utility and helper functions and data. - */ - -#pragma once - -#include "orb_topics.h" - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, - uint8_t *mavlink_state, uint8_t *mavlink_mode); From 323b90bfd9f3a1524f36c089d532ed3836cc2ea3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 23:44:51 +0400 Subject: [PATCH 44/99] mavlink: uORB topics includes moved to mavlink_messages.cpp, more messages implemented --- src/modules/mavlink/mavlink_main.h | 28 +- src/modules/mavlink/mavlink_messages.cpp | 433 ++++++++++++++++------- 2 files changed, 300 insertions(+), 161 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 049f5fedd5..41e781ee8f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -42,44 +42,18 @@ #include #include -#include #include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #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 { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9676068419..3634acefa4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -10,12 +10,33 @@ #include #include +#include +#include +#include +#include #include +#include #include #include +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "mavlink_messages.h" @@ -128,9 +149,9 @@ public: private: MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; struct position_setpoint_triplet_s *pos_sp_triplet; protected: @@ -177,7 +198,6 @@ public: private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; protected: @@ -210,6 +230,10 @@ protected: 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"; @@ -222,13 +246,12 @@ public: private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; - uint32_t accel_counter = 0; - uint32_t gyro_counter = 0; - uint32_t mag_counter = 0; - uint32_t baro_counter = 0; + uint32_t accel_counter; + uint32_t gyro_counter; + uint32_t mag_counter; + uint32_t baro_counter; protected: void subscribe(Mavlink *mavlink) @@ -292,7 +315,6 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: @@ -313,6 +335,75 @@ protected: }; +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), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s)); + armed = (struct actuator_armed_s *)armed_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s)); + act = (struct actuator_controls_s *)act_sub->get_data(); + + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s)); + 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() @@ -638,11 +729,203 @@ protected: }; +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), sizeof(position_setpoint_triplet_s)); + 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), sizeof(vehicle_local_position_setpoint_s)); + 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), sizeof(vehicle_attitude_setpoint_s)); + 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), sizeof(vehicle_rates_setpoint_s)); + 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), sizeof(struct rc_input_values)); + 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); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), @@ -652,6 +935,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamServoOutputRaw(2), new MavlinkStreamServoOutputRaw(3), new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), nullptr }; @@ -705,132 +993,9 @@ MavlinkStream *streams_list[] = { // l->listener->attitude_counter++; //} // -//void -//MavlinkOrbListener::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), l->mavlink->get_subs()->gps_sub, &gps); -// -// /* update SAT info every 10 seconds */ -// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { -// mavlink_msg_gps_status_send(l->mavlink->get_chan(), -// gps.satellites_visible, -// gps.satellite_prn, -// gps.satellite_used, -// gps.satellite_elevation, -// gps.satellite_azimuth, -// gps.satellite_snr); -// } -// -// l->listener->gps_counter++; -//} // // -//void -//MavlinkOrbListener::l_input_rc(const struct listener *l) -//{ -// /* copy rc _mavlink->get_chan()nels into local buffer */ -// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); // -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// -// const unsigned port_width = 8; -// -// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), -// l->listener->rc_raw.timestamp_publication / 1000, -// i, -// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, -// l->listener->rc_raw.rssi); -// } -// } -//} -// -// -//void -//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -//{ -// struct position_setpoint_triplet_s triplet; -// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); -// -// if (!triplet.current.valid) -// return; -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), -// 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 -//MavlinkOrbListener::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), l->mavlink->get_subs()->spl_sub, &local_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), -// MAV_FRAME_LOCAL_NED, -// local_sp.x, -// local_sp.y, -// local_sp.z, -// local_sp.yaw); -//} -// -//void -//MavlinkOrbListener::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), l->mavlink->get_subs()->spa_sub, &att_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), -// att_sp.timestamp / 1000, -// att_sp.roll_body, -// att_sp.pitch_body, -// att_sp.yaw_body, -// att_sp.thrust); -//} -// -//void -//MavlinkOrbListener::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), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), -// rates_sp.timestamp / 1000, -// rates_sp.roll, -// rates_sp.pitch, -// rates_sp.yaw, -// rates_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_actuator_armed(const struct listener *l) -//{ -// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -//} // //void //MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) From 77d1989abae9d267bb74f86fb0104dbefcbcd52a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:06:30 +0400 Subject: [PATCH 45/99] mavlink: more message streams implemented --- src/modules/mavlink/mavlink_messages.cpp | 144 ++++++++++++----------- 1 file changed, 78 insertions(+), 66 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3634acefa4..8097ecdb36 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -335,6 +335,45 @@ protected: }; +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), sizeof(struct vehicle_attitude_s)); + 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() @@ -920,11 +959,49 @@ protected: }; +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), sizeof(struct manual_control_setpoint_s)); + 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); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamAttitudeQuaternion(), new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), @@ -940,6 +1017,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRollPitchYawThrustSetpoint(), new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRCChannelsRaw(), + new MavlinkStreamManualControl(), nullptr }; @@ -949,71 +1027,11 @@ MavlinkStream *streams_list[] = { -//void -//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) -//{ -// /* copy attitude data into local buffer */ -// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send sensor values */ -// mavlink_msg_attitude_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// l->listener->att.roll, -// l->listener->att.pitch, -// l->listener->att.yaw, -// l->listener->att.rollspeed, -// l->listener->att.pitchspeed, -// l->listener->att.yawspeed); -// -// /* limit VFR message rate to 10Hz */ -// hrt_abstime t = hrt_absolute_time(); -// if (t >= l->listener->last_sent_vfr + 100000) { -// l->listener->last_sent_vfr = t; -// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); -// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; -// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; -// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); -// } -// -// /* send quaternion values if it exists */ -// if(l->listener->att.q_valid) { -// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// l->listener->att.q[0], -// l->listener->att.q[1], -// l->listener->att.q[2], -// l->listener->att.q[3], -// l->listener->att.rollspeed, -// l->listener->att.pitchspeed, -// l->listener->att.yawspeed); -// } -// } -// -// l->listener->attitude_counter++; -//} // // // // // -//void -//MavlinkOrbListener::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), l->mavlink->get_subs()->man_control_sp_sub, &man_control); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_manual_control_send(l->mavlink->get_chan(), -// mavlink_system.sysid, -// man_control.roll * 1000, -// man_control.pitch * 1000, -// man_control.yaw * 1000, -// man_control.throttle * 1000, -// 0); -//} // //void //MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) @@ -1069,12 +1087,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_airspeed(const struct listener *l) -//{ -// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -//} -// -//void //MavlinkOrbListener::l_nav_cap(const struct listener *l) //{ // From 836f7c435fe31572e45333877142dce8b4d2fc78 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:16:51 +0400 Subject: [PATCH 46/99] mavlink: code style and copyright fixes --- src/modules/mavlink/mavlink_main.cpp | 448 +++++++++------- src/modules/mavlink/mavlink_main.h | 35 +- src/modules/mavlink/mavlink_messages.cpp | 495 ++++++++++-------- src/modules/mavlink/mavlink_messages.h | 41 +- .../mavlink/mavlink_orb_subscription.cpp | 44 +- .../mavlink/mavlink_orb_subscription.h | 44 +- src/modules/mavlink/mavlink_rate_limiter.cpp | 44 +- src/modules/mavlink/mavlink_rate_limiter.h | 44 +- src/modules/mavlink/mavlink_receiver.cpp | 33 +- src/modules/mavlink/mavlink_receiver.h | 4 +- src/modules/mavlink/mavlink_stream.cpp | 42 +- src/modules/mavlink/mavlink_stream.h | 44 +- 12 files changed, 867 insertions(+), 451 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 672daf641b..c977865538 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin */ #include @@ -91,7 +92,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz -static Mavlink* _head = nullptr; +static Mavlink *_head = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; @@ -112,40 +113,47 @@ 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); + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); break; - case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); + + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); break; - case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); + + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); break; - case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); + + 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); +#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); +#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); +#endif +#ifdef MAVLINK_COMM_6 + + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); break; - #endif +#endif } ssize_t desired = (sizeof(uint8_t) * length); ssize_t ret = write(uart, ch, desired); - if (ret != desired) + if (ret != desired) { warn("write err"); + } } @@ -168,7 +176,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; - fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); } @@ -206,8 +214,9 @@ int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; + while (inst != nullptr) { inst = inst->next; inst_index++; @@ -225,14 +234,17 @@ Mavlink::new_instance() /* create the first instance at _head */ if (::_head == nullptr) { ::_head = inst; - /* afterwards follow the next and append the instance */ + /* 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; } @@ -241,6 +253,7 @@ Mavlink::get_instance(unsigned instance) { Mavlink *inst = ::_head; unsigned inst_index = 0; + while (inst->next != nullptr && inst_index < instance) { inst = inst->next; inst_index++; @@ -277,6 +290,7 @@ Mavlink::destroy_all_instances() unsigned iterations = 0; warnx("waiting for instances to stop"); + while (next_inst != nullptr) { inst_to_del = next_inst; @@ -284,6 +298,7 @@ Mavlink::destroy_all_instances() /* set flag to stop thread and wait for all threads to finish */ inst_to_del->_task_should_exit = true; + while (inst_to_del->thread_running) { printf("."); usleep(10000); @@ -294,6 +309,7 @@ Mavlink::destroy_all_instances() return ERROR; } } + delete inst_to_del; } @@ -308,23 +324,29 @@ Mavlink::destroy_all_instances() bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; + while (inst != nullptr) { /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->device_name)) + 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) + Mavlink *inst = get_instance(index); + + if (inst) { return inst->get_uart_fd(); + } return -1; } @@ -353,21 +375,23 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; + const char *txt = (const char *)arg; // printf("logmsg: %s\n", txt); - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink* inst = ::_head; - while (inst != nullptr) { + Mavlink *inst = ::_head; - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; - inst = inst->next; + while (inst != nullptr) { + mavlink_logbuffer_write(&inst->lb, &msg); + inst->total_counter++; + inst = inst->next; + + } + + return OK; } - return OK; - } default: return ENOTTY; @@ -582,7 +606,7 @@ int Mavlink::mavlink_pm_send_param_for_name(const char *name) int Mavlink::mavlink_pm_send_param(param_t param) { - if (param == PARAM_INVALID) return 1; + if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; @@ -714,38 +738,40 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_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: + 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_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; + 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; + 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->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; @@ -762,25 +788,27 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct 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; + 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->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; @@ -817,7 +845,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 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); + if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } } /* @@ -846,7 +874,8 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (_verbose) warnx("ERROR: index out of bounds"); + + if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -862,7 +891,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin 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); + 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) @@ -870,11 +899,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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; } @@ -892,10 +922,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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); + 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); + + if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } } @@ -910,11 +942,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u 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); + 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"); + + if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -935,7 +968,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t 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); + if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } } void Mavlink::mavlink_waypoint_eventloop(uint64_t now) @@ -945,7 +978,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) 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); + 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; @@ -960,7 +993,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) switch (msg->msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: { + case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); @@ -977,13 +1010,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (_verbose) warnx("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: { + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); @@ -995,30 +1029,33 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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"); + + 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"); + + 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"); + + if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); @@ -1027,14 +1064,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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"); + if (_verbose) { warnx("No waypoints send"); } } wpm->current_count = wpm->size; @@ -1042,17 +1079,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (_verbose) warnx("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"); + + if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST: { + case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); @@ -1062,22 +1102,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + + 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 + /* + * 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); + 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"); + + if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } + break; } @@ -1085,22 +1131,26 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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); - + 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); + + 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); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); } + break; } @@ -1109,11 +1159,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq < wpm->size) { - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); + 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); + + if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } } @@ -1122,18 +1173,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + + 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } + break; } - case MAVLINK_MSG_ID_MISSION_COUNT: { + case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); @@ -1143,18 +1197,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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"); + + 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); + + 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; @@ -1168,24 +1225,31 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + + 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); + + 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"); + 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; - case MAVLINK_MSG_ID_MISSION_ITEM: { + case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); @@ -1200,11 +1264,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; - } + 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) { @@ -1239,6 +1304,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; @@ -1260,13 +1326,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + + 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; @@ -1280,13 +1346,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; } - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); @@ -1305,21 +1372,24 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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"); + + 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"); + + if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1352,8 +1422,9 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) while (i < len - 1) { statustext.text[i] = string[i]; - if (string[i] == '\0') + if (string[i] == '\0') { break; + } i++; } @@ -1411,6 +1482,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) LL_DELETE(_streams, stream); delete stream; } + return OK; } } @@ -1466,18 +1538,22 @@ Mavlink::task_main(int argc, char *argv[]) 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': @@ -1501,6 +1577,7 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(optarg, "custom") == 0) { _mode = MODE_CUSTOM; } + break; case 'v': @@ -1533,35 +1610,41 @@ Mavlink::task_main(int argc, char *argv[]) /* 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; + 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; + 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); @@ -1576,8 +1659,9 @@ Mavlink::task_main(int argc, char *argv[]) /* default values for arguments */ _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); - if (_uart < 0) + if (_uart < 0) { err(1, "could not open %s", device_name); + } /* create the device node that's used for sending text log messages, etc. */ if (instance_count() == 1) { @@ -1615,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("HEARTBEAT", 1.0f); - switch(_mode) { + switch (_mode) { case MODE_OFFBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); @@ -1660,10 +1744,12 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(t)) { /* switch HIL mode if required */ - if (status->hil_state == HIL_STATE_ON) + if (status->hil_state == HIL_STATE_ON) { set_hil_enabled(true); - else if (status->hil_state == HIL_STATE_OFF) + + } else if (status->hil_state == HIL_STATE_OFF) { set_hil_enabled(false); + } } MavlinkStream *stream; @@ -1677,13 +1763,14 @@ Mavlink::task_main(int argc, char *argv[]) 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 (_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); @@ -1740,11 +1827,11 @@ Mavlink::start(int argc, char *argv[]) sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // while (!this->is_running()) { // usleep(200); @@ -1775,7 +1862,7 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::status() { warnx("running"); } @@ -1799,9 +1886,11 @@ Mavlink::stream(int argc, char *argv[]) switch (ch) { case 'r': rate = strtod(optarg, nullptr); + if (rate < 0.0f) { err_flag = true; } + break; case 'd': @@ -1820,6 +1909,7 @@ Mavlink::stream(int argc, char *argv[]) if (!err_flag && rate >= 0.0 && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); + if (inst != nullptr) { if (OK == inst->configure_stream(stream_name, rate)) { if (rate > 0.0f) { @@ -1865,15 +1955,15 @@ int mavlink_main(int argc, char *argv[]) } 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], "status")) { + // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream(argc, argv); - } else { - usage(); - } + } else { + usage(); + } return 0; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 41e781ee8f..532c9bcee3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -36,6 +36,7 @@ * MAVLink 1.0 protocol interface definition. * * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -55,24 +56,24 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" - // FIXME XXX - TO BE MOVED TO XML +// 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 + 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 + 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 }; @@ -144,7 +145,7 @@ public: const char *device_name; enum MAVLINK_MODE { - MODE_CUSTOM=0, + MODE_CUSTOM = 0, MODE_OFFBOARD, MODE_ONBOARD, MODE_HIL @@ -186,7 +187,7 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - Mavlink* next; + Mavlink *next; private: int _mavlink_fd; @@ -233,7 +234,7 @@ private: unsigned int mavlink_param_queue_index; bool mavlink_link_termination_allowed; - + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8097ecdb36..820faae1cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,8 +1,42 @@ -/* - * mavlink_messages.cpp +/**************************************************************************** * - * Created on: 25.02.2014 - * Author: ton + * 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 + * @author Anton Babushkin */ #include @@ -43,7 +77,7 @@ 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); + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); uint16_t cm_uint16_from_m_float(float m) @@ -59,7 +93,7 @@ cm_uint16_from_m_float(float m) } 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) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -72,7 +106,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -81,34 +115,44 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set 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 */ + /* 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; } @@ -118,24 +162,30 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* 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 + || 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 { +class MavlinkStreamHeartbeat : public MavlinkStream +{ public: const char *get_name() { @@ -164,7 +214,8 @@ protected: pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); pos_sp_triplet_sub->update(t); @@ -184,7 +235,8 @@ protected: }; -class MavlinkStreamSysStatus : public MavlinkStream { +class MavlinkStreamSysStatus : public MavlinkStream +{ public: const char *get_name() { @@ -207,28 +259,30 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + 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 { +class MavlinkStreamHighresIMU : public MavlinkStream +{ public: MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) { @@ -260,7 +314,8 @@ protected: sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { sensor_sub->update(t); uint16_t fields_updated = 0; @@ -290,18 +345,19 @@ protected: } 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); + 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 { +class MavlinkStreamAttitude : public MavlinkStream +{ public: const char *get_name() { @@ -324,18 +380,20 @@ protected: att = (struct vehicle_attitude_s *)att_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); } }; -class MavlinkStreamAttitudeQuaternion : public MavlinkStream { +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ public: const char *get_name() { @@ -358,23 +416,25 @@ protected: att = (struct vehicle_attitude_s *)att_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + 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 { +class MavlinkStreamVFRHUD : public MavlinkStream +{ public: const char *get_name() { @@ -421,7 +481,8 @@ protected: airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); pos_sub->update(t); armed_sub->update(t); @@ -433,17 +494,18 @@ protected: 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); + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); } }; -class MavlinkStreamGPSRawInt : public MavlinkStream { +class MavlinkStreamGPSRawInt : public MavlinkStream +{ public: const char *get_name() { @@ -466,25 +528,27 @@ protected: gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + 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 { +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ public: const char *get_name() { @@ -513,25 +577,27 @@ protected: home = (struct home_position_s *)home_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + 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 { +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ public: const char *get_name() { @@ -554,22 +620,24 @@ protected: pos = (struct vehicle_local_position_s *)pos_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } }; -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ public: const char *get_name() { @@ -592,18 +660,20 @@ protected: home = (struct home_position_s *)home_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); } }; -class MavlinkStreamServoOutputRaw : public MavlinkStream { +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ public: MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) { @@ -641,25 +711,27 @@ protected: act = (struct actuator_outputs_s *)act_sub->get_data(); } - void send(const hrt_abstime t) { + 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]); + 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 { +class MavlinkStreamHILControls : public MavlinkStream +{ public: const char *get_name() { @@ -694,7 +766,8 @@ protected: act = (struct actuator_outputs_s *)act_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); pos_sp_triplet_sub->update(t); act_sub->update(t); @@ -710,65 +783,66 @@ protected: /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->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(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->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(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + ((act->output[6] - 900.0f) / 600.0f) / 2.0f, + ((act->output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); } else { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + hrt_absolute_time(), + (act->output[0] - 1500.0f) / 500.0f, + (act->output[1] - 1500.0f) / 500.0f, + (act->output[2] - 1500.0f) / 500.0f, + (act->output[3] - 1000.0f) / 1000.0f, + (act->output[4] - 1500.0f) / 500.0f, + (act->output[5] - 1500.0f) / 500.0f, + (act->output[6] - 1500.0f) / 500.0f, + (act->output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); } } }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream { +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ public: const char *get_name() { @@ -791,7 +865,8 @@ protected: pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sp_triplet_sub->update(t); mavlink_msg_global_position_setpoint_int_send(_channel, @@ -804,7 +879,8 @@ protected: }; -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -827,7 +903,8 @@ protected: pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sp_sub->update(t); mavlink_msg_local_position_setpoint_send(_channel, @@ -840,7 +917,8 @@ protected: }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream { +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -863,7 +941,8 @@ protected: att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sp_sub->update(t); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, @@ -876,7 +955,8 @@ protected: }; -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream { +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -899,7 +979,8 @@ protected: att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_rates_sp_sub->update(t); mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, @@ -912,7 +993,8 @@ protected: }; -class MavlinkStreamRCChannelsRaw : public MavlinkStream { +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ public: const char *get_name() { @@ -935,7 +1017,8 @@ protected: rc = (struct rc_input_values *)rc_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { rc_sub->update(t); const unsigned port_width = 8; @@ -943,23 +1026,24 @@ protected: 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); + 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 { +class MavlinkStreamManualControl : public MavlinkStream +{ public: const char *get_name() { @@ -982,43 +1066,44 @@ protected: manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); } - void send(const hrt_abstime t) { + 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); + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); } }; 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(), - nullptr + 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(), + nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 3dc6cb6998..b8823263a8 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -1,8 +1,41 @@ -/* - * mavlink_messages.h +/**************************************************************************** * - * Created on: 25.02.2014 - * Author: ton + * 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.h + * MAVLink 1.0 message formatters definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_MESSAGES_H_ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 35470a19a1..e1208bca9b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -1,10 +1,42 @@ -/* - * mavlink_orb_subscription.cpp +/**************************************************************************** * - * Created on: 23.02.2014 - * Author: ton - */ + * 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.cpp + * uORB subscription implementation. + * + * @author Anton Babushkin + */ #include #include @@ -45,10 +77,12 @@ MavlinkOrbSubscription::update(const hrt_abstime t) _last_check = t; bool updated; orb_check(_fd, &updated); + if (updated) { orb_copy(_topic, _fd, _data); return true; } } + return false; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 79ff3abdbe..3cf33ccef2 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -1,8 +1,41 @@ -/* - * mavlink_orb_subscription.h +/**************************************************************************** * - * Created on: 23.02.2014 - * Author: ton + * 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 */ #ifndef MAVLINK_ORB_SUBSCRIPTION_H_ @@ -12,7 +45,8 @@ #include -class MavlinkOrbSubscription { +class MavlinkOrbSubscription +{ public: MavlinkOrbSubscription *next; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index f5bb06ccd2..f6ed6e6627 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -1,10 +1,42 @@ -/* - * mavlink_rate_limiter.cpp +/**************************************************************************** * - * Created on: 26.02.2014 - * Author: ton - */ + * 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 + */ #include "mavlink_rate_limiter.h" @@ -30,9 +62,11 @@ 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/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h index 6db65f638f..0b37538e62 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.h +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -1,8 +1,41 @@ -/* - * mavlink_rate_limiter.h +/**************************************************************************** * - * Created on: 26.02.2014 - * Author: ton + * 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.h + * Message rate limiter definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_RATE_LIMITER_H_ @@ -11,7 +44,8 @@ #include -class MavlinkRateLimiter { +class MavlinkRateLimiter +{ private: hrt_abstime _last_sent; hrt_abstime _interval; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a3546e954b..f85773ae02 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -566,8 +566,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; /* go back to -PI..PI */ - if (heading_rad > M_PI_F) + 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 @@ -606,7 +607,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - // publish global position + // 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 @@ -627,10 +628,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (pub_hil_local_pos > 0) { float x; float y; - bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? - double lat = hil_state.lat*1e-7; - double lon = hil_state.lon*1e-7; - map_projection_project(lat, lon, &x, &y); + bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // 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; @@ -638,10 +639,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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.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; @@ -651,6 +652,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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; @@ -661,12 +663,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb = q.to_dcm(); + math::Matrix<3, 3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) { hil_attitude.R[i][j] = C_nb(i, j); + } hil_attitude.R_valid = true; @@ -841,9 +844,9 @@ void MavlinkReceiver::print_status() } -void * MavlinkReceiver::start_helper(void *context) +void *MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); return rcv->receive_thread(NULL); } @@ -865,7 +868,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fca5de9179..199e426892 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -97,13 +97,13 @@ public: static pthread_t receive_start(Mavlink *parent); - static void * start_helper(void *context); + static void *start_helper(void *context); private: perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink* _mavlink; + Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); void *receive_thread(void *arg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 703f74b4c3..8694950985 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -1,8 +1,41 @@ -/* - * mavlink_stream.cpp +/**************************************************************************** * - * Created on: 24.02.2014 - * Author: ton + * 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 */ #include @@ -43,6 +76,7 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { /* interval expired, send message */ send(t); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 9f175adbe5..135e1bce08 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -1,8 +1,41 @@ -/* - * mavlink_stream.h +/**************************************************************************** * - * Created on: 24.02.2014 - * Author: ton + * 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 */ #ifndef MAVLINK_STREAM_H_ @@ -15,7 +48,8 @@ class MavlinkStream; #include "mavlink_main.h" -class MavlinkStream { +class MavlinkStream +{ private: hrt_abstime _last_sent; From 1b8004cd8ecf7824584aac9e7fed447714feb716 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 16:43:04 +0400 Subject: [PATCH 47/99] mavlink: add new streams in main loop, minor cleanup and fixes --- ROMFS/px4fmu_common/init.d/rcS | 36 +++++---- src/modules/mavlink/mavlink_main.cpp | 76 +++++++++++++++---- src/modules/mavlink/mavlink_main.h | 6 +- src/modules/mavlink/mavlink_messages.cpp | 52 ++++++------- .../mavlink/mavlink_orb_subscription.cpp | 9 ++- .../mavlink/mavlink_orb_subscription.h | 4 +- 6 files changed, 121 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c3065b6fc8..17f7dd077d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -117,6 +117,7 @@ then set PWM_MAX none set MKBLCTRL_MODE none set FMU_MODE pwm + set MAVLINK_FLAGS default set MAV_TYPE none # @@ -381,26 +382,33 @@ then # 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 -r 1000 -d /dev/ttyS0 + sleep 1 + set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0" usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start -r 1000 - 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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c977865538..b996413a85 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -171,6 +171,9 @@ Mavlink::Mavlink() : _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")) @@ -301,6 +304,7 @@ Mavlink::destroy_all_instances() while (inst_to_del->thread_running) { printf("."); + fflush(stdout); usleep(10000); iterations++; @@ -1443,7 +1447,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1456,7 +1460,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, cons } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); LL_APPEND(_subscriptions, sub_new); @@ -1509,6 +1513,34 @@ Mavlink::configure_stream(const char *stream_name, const float rate) 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[]) { @@ -1686,8 +1718,8 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); - MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); + 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(); @@ -1726,6 +1758,9 @@ Mavlink::task_main(int argc, char *argv[]) break; } + /* don't send parameters on startup without request */ + mavlink_param_queue_index = param_count(); + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); @@ -1752,6 +1787,24 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* 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); @@ -1794,6 +1847,9 @@ Mavlink::task_main(int argc, char *argv[]) perf_end(_loop_perf); } + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + /* wait for threads to complete */ pthread_join(receive_thread, NULL); @@ -1911,17 +1967,7 @@ Mavlink::stream(int argc, char *argv[]) Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { - if (OK == inst->configure_stream(stream_name, rate)) { - if (rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", stream_name, device_name, rate); - - } else { - warnx("stream %s on device %s disabled", stream_name, device_name); - } - - } else { - warnx("stream %s not found", stream_name, device_name); - } + inst->configure_stream_threadsafe(stream_name, rate); } else { errx(1, "mavlink for device %s is not running", device_name); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 532c9bcee3..ebea53d52e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -180,7 +180,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); mavlink_channel_t get_channel(); @@ -235,6 +235,9 @@ private: bool mavlink_link_termination_allowed; + char * _subscribe_to_stream; + float _subscribe_to_stream_rate; + /** * Send one parameter. * @@ -296,6 +299,7 @@ private: 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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 820faae1cc..8be7d76d70 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -207,10 +207,10 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + 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), sizeof(position_setpoint_triplet_s)); + 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(); } @@ -255,7 +255,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); } @@ -310,7 +310,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } @@ -376,7 +376,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); } @@ -412,7 +412,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); } @@ -465,19 +465,19 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + 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), sizeof(struct vehicle_global_position_s)); + 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), sizeof(struct actuator_armed_s)); + 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), sizeof(struct actuator_controls_s)); + 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), sizeof(struct airspeed_s)); + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } @@ -524,7 +524,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); + gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } @@ -570,10 +570,10 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + 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), sizeof(struct home_position_s)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); home = (struct home_position_s *)home_sub->get_data(); } @@ -616,7 +616,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); pos = (struct vehicle_local_position_s *)pos_sub->get_data(); } @@ -656,7 +656,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); home = (struct home_position_s *)home_sub->get_data(); } @@ -707,7 +707,7 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s)); + act_sub = mavlink->add_orb_subscription(act_topics[_n]); act = (struct actuator_outputs_s *)act_sub->get_data(); } @@ -756,13 +756,13 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + 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), sizeof(position_setpoint_triplet_s)); + 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), sizeof(struct actuator_outputs_s)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); } @@ -861,7 +861,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + 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(); } @@ -899,7 +899,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint), sizeof(vehicle_local_position_setpoint_s)); + 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(); } @@ -937,7 +937,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint), sizeof(vehicle_attitude_setpoint_s)); + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); } @@ -975,7 +975,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint), sizeof(vehicle_rates_setpoint_s)); + 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(); } @@ -1013,7 +1013,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc), sizeof(struct rc_input_values)); + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); rc = (struct rc_input_values *)rc_sub->get_data(); } @@ -1062,7 +1062,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint), sizeof(struct manual_control_setpoint_s)); + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); } diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index e1208bca9b..6279e5366a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -42,13 +42,14 @@ #include #include #include +#include #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) { - _data = malloc(size); - memset(_data, 0, size); + _data = malloc(topic->o_size); + memset(_data, 0, topic->o_size); _fd = orb_subscribe(_topic); } @@ -58,7 +59,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(_data); } -const struct orb_metadata * +const orb_id_t MavlinkOrbSubscription::get_topic() { return _topic; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 3cf33ccef2..eacc270349 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,12 +50,12 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; - MavlinkOrbSubscription(const orb_id_t topic, size_t size); + MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); void *get_data(); - const struct orb_metadata *get_topic(); + const orb_id_t get_topic(); private: const orb_id_t _topic; From c10ef787539265bc36fb76c855aa19b30ea24b04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 17:12:46 +0400 Subject: [PATCH 48/99] mavlink: stop fixes --- src/modules/mavlink/mavlink_main.cpp | 11 ++++++++--- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b996413a85..568666c1ea 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -308,7 +308,7 @@ Mavlink::destroy_all_instances() usleep(10000); iterations++; - if (iterations > 10000) { + if (iterations > 1000) { warnx("ERROR: Couldn't stop all mavlink instances."); return ERROR; } @@ -1850,18 +1850,23 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; + warnv("waiting for UART receive thread"); + /* wait for threads to complete */ pthread_join(receive_thread, NULL); - /* Reset the UART flags to original state */ + /* reset the UART flags to original state */ tcsetattr(_uart, TCSANOW, &uart_config_original); + /* close UART */ + close(_uart); + /* destroy log buffer */ mavlink_logbuffer_destroy(&lb); thread_running = false; - warnx("exiting."); + warnx("exiting"); _mavlink_task = -1; _exit(0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f85773ae02..b6e008cbfc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::receive_thread(void *arg) { int uart_fd = _mavlink->get_uart_fd(); - const int timeout = 1000; + const int timeout = 500; uint8_t buf[32]; mavlink_message_t msg; From 256cc2b411b1f36397884bfd019b9ac3e4cd1850 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 18:30:30 +0400 Subject: [PATCH 49/99] mavlink: cleanup and refactoring, rcS: EXIT_ON_END fix --- ROMFS/px4fmu_common/init.d/rcS | 3 +- src/modules/mavlink/mavlink_main.cpp | 368 ++++++++++++--------------- src/modules/mavlink/mavlink_main.h | 25 +- 3 files changed, 179 insertions(+), 217 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 17f7dd077d..c96f44e023 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -118,6 +118,7 @@ then set MKBLCTRL_MODE none set FMU_MODE pwm set MAVLINK_FLAGS default + set EXIT_ON_END no set MAV_TYPE none # @@ -380,7 +381,6 @@ then # # MAVLink # - set EXIT_ON_END no if [ $MAVLINK_FLAGS == default ] then @@ -539,6 +539,7 @@ then if [ $EXIT_ON_END == yes ] then + echo "[init] Exit from nsh" exit fi diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 568666c1ea..99bef1f731 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -92,7 +92,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz -static Mavlink *_head = nullptr; +static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; @@ -160,35 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - device_name(DEFAULT_DEVICE_NAME), - _task_should_exit(false), next(nullptr), + _device_name(DEFAULT_DEVICE_NAME), + _task_should_exit(false), _mavlink_fd(-1), - thread_running(false), - _mavlink_task(-1), - _mavlink_incoming_fd(-1), + _task_running(false), _mavlink_hil_enabled(false), _subscriptions(nullptr), _streams(nullptr), - mission_pub(-1), - mavlink_param_queue_index(0), + _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; + _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; - // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); - } Mavlink::~Mavlink() { - if (_mavlink_task != -1) { - - /* task wakes up every 100ms or so at the longest */ + 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 */ @@ -200,10 +195,11 @@ Mavlink::~Mavlink() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_mavlink_task); + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); break; } - } while (_mavlink_task != -1); + } while (_task_running); } } @@ -216,12 +212,10 @@ Mavlink::set_mode(enum MAVLINK_MODE mode) int Mavlink::instance_count() { - /* note: a local buffer count will help if this ever is called often */ - Mavlink *inst = ::_head; unsigned inst_index = 0; + Mavlink *inst; - while (inst != nullptr) { - inst = inst->next; + LL_FOREACH(::_mavlink_instances, inst) { inst_index++; } @@ -232,11 +226,11 @@ Mavlink * Mavlink::new_instance() { Mavlink *inst = new Mavlink(); - Mavlink *next = ::_head; + Mavlink *next = ::_mavlink_instances; /* create the first instance at _head */ - if (::_head == nullptr) { - ::_head = inst; + if (::_mavlink_instances == nullptr) { + ::_mavlink_instances = inst; /* afterwards follow the next and append the instance */ } else { @@ -254,19 +248,17 @@ Mavlink::new_instance() Mavlink * Mavlink::get_instance(unsigned instance) { - Mavlink *inst = ::_head; + Mavlink *inst; unsigned inst_index = 0; + LL_FOREACH(::_mavlink_instances, inst) { + if (instance == inst_index) { + return inst; + } - while (inst->next != nullptr && inst_index < instance) { - inst = inst->next; inst_index++; } - if (inst_index < instance) { - inst = nullptr; - } - - return inst; + return nullptr; } Mavlink * @@ -274,8 +266,8 @@ Mavlink::get_instance_for_device(const char *device_name) { Mavlink *inst; - LL_FOREACH(::_head, inst) { - if (strcmp(inst->device_name, device_name) == 0) { + LL_FOREACH(::_mavlink_instances, inst) { + if (strcmp(inst->_device_name, device_name) == 0) { return inst; } } @@ -288,21 +280,20 @@ Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; - Mavlink *next_inst = ::_head; + 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->thread_running) { + while (inst_to_del->_task_running) { printf("."); fflush(stdout); usleep(10000); @@ -318,7 +309,7 @@ Mavlink::destroy_all_instances() } /* reset head */ - ::_head = nullptr; + ::_mavlink_instances = nullptr; printf("\n"); warnx("all instances stopped"); @@ -328,12 +319,12 @@ Mavlink::destroy_all_instances() bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { - Mavlink *inst = ::_head; + Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->device_name)) { + if (inst != self && !strcmp(device_name, inst->_device_name)) { return true; } @@ -358,7 +349,7 @@ Mavlink::get_uart_fd(unsigned index) int Mavlink::get_uart_fd() { - return _uart; + return _uart_fd; } mavlink_channel_t @@ -384,12 +375,12 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink *inst = ::_head; + Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; inst = inst->next; } @@ -493,7 +484,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* open uart */ - _uart = open(uart_name, O_RDWR | O_NOCTTY); + _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ struct termios uart_config; @@ -501,14 +492,14 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * *is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(_uart, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(_uart); + close(_uart_fd); return -1; } /* Fill the struct for the new configuration */ - tcgetattr(_uart, &uart_config); + tcgetattr(_uart_fd, &uart_config); /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -519,19 +510,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* 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); + close(_uart_fd); return -1; } } - if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(_uart); + close(_uart_fd); return -1; } - return _uart; + return _uart_fd; } int @@ -583,9 +574,9 @@ 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++; + 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 { @@ -595,7 +586,7 @@ int Mavlink::mavlink_pm_queued_send() void Mavlink::mavlink_pm_start_queued_send() { - mavlink_param_queue_index = 0; + _mavlink_param_queue_index = 0; } int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) @@ -730,11 +721,11 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); + if (_mission_pub < 0) { + _mission_pub = orb_advertise(ORB_ID(mission), &mission); } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); + orb_publish(ORB_ID(mission), _mission_pub, &mission); } } @@ -863,7 +854,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 */ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) { - if (seq < wpm->size) { + if (seq < _wpm->size) { mavlink_message_t msg; mavlink_mission_current_t wpc; @@ -872,7 +863,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t 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) { + } else if (seq == 0 && _wpm->size == 0) { /* don't broadcast if no WPs */ @@ -906,7 +897,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t dm_item_t dm_current; - if (wpm->current_dataman_id == 0) { + if (_wpm->current_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { @@ -929,7 +920,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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); + 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); } } @@ -937,7 +928,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) { - if (seq < wpm->max_size) { + if (seq < _wpm->max_size) { mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; @@ -978,15 +969,15 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t 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) { + 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); } + 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; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_partner_sysid = 0; + _wpm->current_partner_compid = 0; } } @@ -1001,14 +992,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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 ((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) { + 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; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; } } @@ -1026,10 +1017,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < wpm->size) { + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < _wpm->size) { mission.current_index = wpc.seq; publish_mission(); @@ -1064,22 +1055,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->size > 0) { + 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; + _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); + _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"); @@ -1100,10 +1091,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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 (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) { + if (wpr.seq >= _wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); @@ -1116,12 +1107,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) * 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 (_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; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); @@ -1131,20 +1122,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == wpm->current_wp_id) { + 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) { + } 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); } + 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; } @@ -1153,20 +1144,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); } + 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; + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - if (wpr.seq < wpm->size) { + if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + 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); + 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); } } @@ -1174,11 +1165,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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)) { + 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); } + 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 { @@ -1196,14 +1187,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } @@ -1217,17 +1208,17 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + _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); + 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) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wpm->current_wp_id == 0) { + 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); } @@ -1235,7 +1226,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } } } else { @@ -1259,14 +1250,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - wpm->timestamp_lastaction = now; + _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 (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); @@ -1274,30 +1265,30 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= wpm->current_count) { + 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); + 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; + _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; + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1305,7 +1296,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) dm_item_t dm_next; - if (wpm->current_dataman_id == 0) { + if (_wpm->current_dataman_id == 0) { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; mission.dataman_id = 1; @@ -1315,8 +1306,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } 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; + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1327,25 +1318,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) // XXX ignore current set mission.current_index = -1; - wpm->current_wp_id = wp.seq + 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 (_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); } + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mission.count = wpm->current_count; + mission.count = _wpm->current_count; publish_mission(); - wpm->current_dataman_id = mission.dataman_id; - wpm->size = wpm->current_count; + _wpm->current_dataman_id = mission.dataman_id; + _wpm->size = _wpm->current_count; - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _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); + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } else { @@ -1363,10 +1354,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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; + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + _wpm->timestamp_lastaction = now; - wpm->size = 0; + _wpm->size = 0; /* prepare mission topic */ mission.dataman_id = -1; @@ -1375,10 +1366,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); } @@ -1389,7 +1380,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + } 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"); @@ -1549,7 +1540,7 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&_logbuffer, 5); int ch; _baudrate = 57600; @@ -1589,7 +1580,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'd': - device_name = optarg; + _device_name = optarg; break; // case 'e': @@ -1636,8 +1627,8 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(device_name, this)) { - errx(1, "mavlink instance for %s already running", device_name); + if (Mavlink::instance_exists(_device_name, this)) { + errx(1, "mavlink instance for %s already running", _device_name); } /* inform about mode */ @@ -1680,7 +1671,7 @@ Mavlink::task_main(int argc, char *argv[]) } warnx("data rate: %d bytes/s", _datarate); - warnx("port: %s, baudrate: %d", device_name, _baudrate); + warnx("port: %s, baudrate: %d", _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1689,16 +1680,14 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; /* default values for arguments */ - _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart); - if (_uart < 0) { - err(1, "could not open %s", device_name); + 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. */ - if (instance_count() == 1) { - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); - } + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1707,16 +1696,16 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - receive_thread = MavlinkReceiver::receive_start(this); + _receive_thread = MavlinkReceiver::receive_start(this); /* initialize waypoint manager */ - mavlink_wpm_init(wpm); + 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)); - thread_running = true; + _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1759,7 +1748,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* don't send parameters on startup without request */ - mavlink_param_queue_index = param_count(); + _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); @@ -1791,15 +1780,16 @@ Mavlink::task_main(int argc, char *argv[]) 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); + 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); + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, device_name); + warnx("stream %s not found", _subscribe_to_stream, _device_name); } + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; } @@ -1834,9 +1824,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (!mavlink_logbuffer_is_empty(&lb)) { + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); + int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); @@ -1850,25 +1840,23 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; - warnv("waiting for UART receive thread"); + warnx("waiting for UART receive thread"); /* wait for threads to complete */ - pthread_join(receive_thread, NULL); + pthread_join(_receive_thread, NULL); /* reset the UART flags to original state */ - tcsetattr(_uart, TCSANOW, &uart_config_original); + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); /* close UART */ - close(_uart); + close(_uart_fd); /* destroy log buffer */ - mavlink_logbuffer_destroy(&lb); - - thread_running = false; + mavlink_logbuffer_destroy(&_logbuffer); warnx("exiting"); - _mavlink_task = -1; + _task_running = false; _exit(0); } @@ -1876,6 +1864,7 @@ 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); } @@ -1887,37 +1876,12 @@ Mavlink::start(int argc, char *argv[]) char buf[32]; sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } + task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); return OK; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ebea53d52e..94bef7e726 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -142,7 +142,7 @@ public: int get_uart_fd(); - const char *device_name; + const char *_device_name; enum MAVLINK_MODE { MODE_CUSTOM = 0, @@ -191,10 +191,7 @@ protected: private: int _mavlink_fd; - bool thread_running; - int _mavlink_task; /**< task handle for sensor task */ - - int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ + bool _task_running; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -204,7 +201,7 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - orb_advert_t mission_pub; + orb_advert_t _mission_pub; struct mission_s mission; uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; @@ -212,17 +209,17 @@ private: uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; - struct mavlink_logbuffer lb; - unsigned int total_counter; + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; - pthread_t receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage wpm_s; - mavlink_wpm_storage *wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; bool _verbose; - int _uart; + int _uart_fd; int _baudrate; int _datarate; @@ -231,11 +228,11 @@ private: * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; bool mavlink_link_termination_allowed; - char * _subscribe_to_stream; + char *_subscribe_to_stream; float _subscribe_to_stream_rate; /** From 6948defdb26711fd8336b5e0173664bf98517406 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 19:27:53 +0400 Subject: [PATCH 50/99] mavlink: HIL fixes, performance optimization --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- src/modules/mavlink/mavlink_main.cpp | 21 +++++++++++++-------- src/modules/mavlink/mavlink_main.h | 4 +++- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be42755..0de2d42958 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -r 10000 -d /dev/ttyACM0 +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 c96f44e023..48532e7af6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then if [ $HIL == yes ] then sleep 1 - set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0" + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 99bef1f731..fb29c9c716 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ 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 +#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate static Mavlink *_mavlink_instances = nullptr; @@ -166,6 +166,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _mavlink_hil_enabled(false), + _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), @@ -1723,7 +1724,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MODE_OFFBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); @@ -1733,14 +1734,15 @@ Mavlink::task_main(int argc, char *argv[]) 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 * rate_mult); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("ATTITUDE", 2.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - configure_stream("HIL_CONTROLS", 20.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); break; default: @@ -1753,9 +1755,12 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.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); + usleep(_main_loop_delay); perf_begin(_loop_perf); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 94bef7e726..b52c12796d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -196,7 +196,9 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* states */ - bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ + bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; From a1ea89ea2d3665709f5249ebd7cc3fbadaca603a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 19:42:29 +0400 Subject: [PATCH 51/99] mavlink: more precise message streams timing --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- src/modules/mavlink/mavlink_rate_limiter.cpp | 4 ++-- src/modules/mavlink/mavlink_stream.cpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fb29c9c716..e6c474aa73 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1462,8 +1462,8 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) int Mavlink::configure_stream(const char *stream_name, const float rate) { - /* calculate interval in ms, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000.0f / rate) : 0; + /* calculate interval in us, 0 means disabled stream */ + unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; /* search if stream exists */ MavlinkStream *stream; @@ -1752,8 +1752,8 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); + 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; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index f6ed6e6627..9cdda8b17c 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -44,7 +44,7 @@ MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000) { } -MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000) +MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval) { } @@ -55,7 +55,7 @@ MavlinkRateLimiter::~MavlinkRateLimiter() void MavlinkRateLimiter::set_interval(unsigned int interval) { - _interval = interval * 1000; + _interval = interval; } bool diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 8694950985..bb19d7e336 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -57,7 +57,7 @@ MavlinkStream::~MavlinkStream() void MavlinkStream::set_interval(const unsigned int interval) { - _interval = interval * 1000; + _interval = interval; } /** From 85001b52aeddaab0f5a1faea8eade8294d447e9f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 20:09:02 +0400 Subject: [PATCH 52/99] mavlink: VFR_HUD message added to default message sets --- src/modules/mavlink/mavlink_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e6c474aa73..c10c95dbdb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1727,10 +1727,10 @@ Mavlink::task_main(int argc, char *argv[]) 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", 5.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - configure_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); break; case MODE_HIL: @@ -1739,6 +1739,7 @@ Mavlink::task_main(int argc, char *argv[]) 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); From 63bdb749adc26550b0cfb6dd8b4f0e0a7173ba10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:41:27 +0400 Subject: [PATCH 53/99] mavlink: unused include removed --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b6e008cbfc..032958b74f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -76,7 +76,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" -// #include "waypoints.h" #include "mavlink_receiver.h" #include "mavlink_main.h" #include "util.h" From 624ff010185c16dffacc8d000aae2748ff1cd0a1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 12:53:46 +0400 Subject: [PATCH 54/99] mavlink: HIL fixes, send 0 when disarmed --- src/modules/mavlink/mavlink_messages.cpp | 88 +++++++++--------------- 1 file changed, 34 insertions(+), 54 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8be7d76d70..c22289772c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -778,65 +778,45 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* HIL message as per MAVLink spec */ + /* 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 */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); + float out[8]; - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); + 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 if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + } 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); } }; From cb8095c5ac932bbf49137491d92b4cf064021058 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 19:56:37 +0400 Subject: [PATCH 55/99] navigator: fixed wrong merge --- src/modules/navigator/navigator_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ff1932c66c..c45cafc1ba 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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); From 3107f4d62cb07de70619093be57ce2b634763eba Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:26:26 +0400 Subject: [PATCH 56/99] mavlink: UART receiver major cleanup --- src/modules/mavlink/mavlink_receiver.cpp | 824 ++++++++++++----------- src/modules/mavlink/mavlink_receiver.h | 57 +- 2 files changed, 444 insertions(+), 437 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 032958b74f..8a80277386 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -82,39 +82,48 @@ __BEGIN_DECLS __END_DECLS +static const float mg2ms2 = 9.8f / 1000.0f; + MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - pub_hil_global_pos(-1), - pub_hil_local_pos(-1), - pub_hil_attitude(-1), - pub_hil_gps(-1), - pub_hil_sensors(-1), - pub_hil_gyro(-1), - pub_hil_accel(-1), - pub_hil_mag(-1), - pub_hil_baro(-1), - pub_hil_airspeed(-1), - pub_hil_battery(-1), - hil_counter(0), - hil_frames(0), - old_timestamp(0), - cmd_pub(-1), - flow_pub(-1), - offboard_control_sp_pub(-1), - vicon_position_pub(-1), - telemetry_status_pub(-1), - lat0(0), - lon0(0), - alt0(0.0) -{ + _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) +{ + memset(&hil_local_pos, 0, sizeof(hil_local_pos)); } void MavlinkReceiver::handle_message(mavlink_message_t *msg) { - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + uint64_t timestamp = hrt_absolute_time(); + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + /* command */ mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); @@ -131,6 +140,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) _mavlink->_task_should_exit = true; } else { + 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; @@ -149,24 +160,25 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) vcmd.confirmation = cmd_mavlink.confirmation; /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } } - } - if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + } else if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + /* 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 = flow.time_usec; + f.timestamp = timestamp; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; @@ -175,21 +187,21 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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); + if (_flow_pub <= 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &f); + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - } - if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* Set mode on request */ + } else 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); + 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 */ @@ -208,21 +220,22 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) vcmd.confirmation = 1; /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + 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); + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } - } - /* Handle Vicon position estimates */ - if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + } else if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + /* vicon */ mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(msg, &pos); - vicon_position.timestamp = hrt_absolute_time(); + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); + vicon_position.timestamp = timestamp; vicon_position.x = pos.x; vicon_position.y = pos.y; @@ -232,21 +245,21 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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); + 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); + 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) { + } else if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + /* offboard control */ 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) { + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); /* switch to a receiving link mode */ //TODO why do we need this? @@ -297,29 +310,25 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) offboard_control_sp.armed = ml_armed; offboard_control_sp.mode = static_cast(ml_mode); - offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.timestamp = timestamp; - /* 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); + 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); + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } } - } - - /* handle status updates of the radio */ - if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { - - struct telemetry_status_s tstatus; + } else if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + /* telemetry status */ mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - /* publish telemetry status topic */ - tstatus.timestamp = hrt_absolute_time(); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = timestamp; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -329,11 +338,59 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub <= 0) { - telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + 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); + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + + } else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + /* manual control */ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + /* rc channels */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); + + rc.timestamp = timestamp; + rc.chan_count = 4; + + 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; + + if (_rc_pub == 0) { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + + } else { + orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); + } + } + + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* 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); + + manual.timestamp = timestamp; + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; + + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } } } @@ -346,270 +403,301 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) */ if (_mavlink->get_hil_enabled()) { - - uint64_t timestamp = hrt_absolute_time(); - if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - + /* HIL sensors */ mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - /* sensors general */ - hil_sensors.timestamp = hrt_absolute_time(); + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - /* 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; + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + 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; + + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } /* 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; + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - /* 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; + 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; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } /* 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; + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + 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; + + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } /* 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; + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); - /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; - /* airspeed from differential pressure, ambient pressure and temp */ - struct airspeed_s airspeed; + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - 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); + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } } - //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); + /* sensor combined */ + { + struct sensor_combined_s hil_sensors; + memset(&hil_sensors, 0, sizeof(hil_sensors)); - /* 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(); + hil_sensors.timestamp = timestamp; - if (pub_hil_gyro < 0) { - pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + 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; - } else { - orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + 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); + } } - struct accel_report accel; + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - accel.x_raw = imu.xacc / mg2ms2; + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - accel.y_raw = imu.yacc / mg2ms2; + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - accel.z_raw = imu.zacc / mg2ms2; - - accel.x = imu.xacc; - - accel.y = imu.yacc; - - accel.z = imu.zacc; - - accel.temperature = imu.temperature; - - accel.timestamp = hrt_absolute_time(); - - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } - struct mag_report mag; + /* increment counters */ + _hil_counter++; + _hil_frames++; - mag.x_raw = imu.xmag / mga2ga; - - mag.y_raw = imu.ymag / mga2ga; - - mag.z_raw = imu.zmag / mga2ga; - - mag.x = imu.xmag; - - mag.y = imu.ymag; - - mag.z = imu.zmag; - - mag.timestamp = hrt_absolute_time(); - - if (pub_hil_mag < 0) { - pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + /* 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; } - struct baro_report baro; - - baro.pressure = imu.abs_pressure; - - baro.altitude = imu.pressure_alt; - - baro.temperature = imu.temperature; - - baro.timestamp = hrt_absolute_time(); - - if (pub_hil_baro < 0) { - pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); - } - - /* publish combined sensor topic */ - if (pub_hil_sensors > 0) { - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - } else { - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - } - - /* 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; - - /* lazily publish the battery voltage */ - if (pub_hil_battery > 0) { - orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); - - } else { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - - // increment counters - hil_counter++; - hil_frames++; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames / 10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - - if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - + } else if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { + /* HIL GPS */ mavlink_hil_gps_t gps; mavlink_msg_hil_gps_decode(msg, &gps); - /* gps */ - hil_gps.timestamp_position = gps.time_usec; + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); + + hil_gps.timestamp_time = timestamp; hil_gps.time_gps_usec = gps.time_usec; + + 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 - hil_gps.timestamp_variance = gps.time_usec; + + 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; + + hil_gps.timestamp_velocity = timestamp; 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.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + + hil_gps.timestamp_satellites = timestamp; 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); + if (_gps_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } else { - pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } - } - - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - + } else if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { + /* 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; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - if (pub_hil_airspeed < 0) { - pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + 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; - } else { - orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } } - // 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 + /* 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); + + } else { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } + + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + hil_global_pos.timestamp = timestamp; hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; @@ -618,19 +706,31 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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; - } else { - pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + if (_global_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + + } else { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } } - // publish local position - if (pub_hil_local_pos > 0) { + /* 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; + } + float x; float y; - bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve? - double lat = hil_state.lat * 1e-7; - double lon = hil_state.lon * 1e-7; - map_projection_project(lat, lon, &x, &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; @@ -638,148 +738,62 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) 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.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; - 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; + + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? 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); - } + if (_local_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); - /* 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(); - - /* 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); + } else { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } - - hil_attitude.R_valid = true; - - /* 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_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; - - /* set timestamp and notify processes (broadcast) */ - hil_attitude.timestamp = hrt_absolute_time(); - - if (pub_hil_attitude > 0) { - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); - - } else { - pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } - struct accel_report accel; + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + 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; - accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - 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; - - accel.timestamp = hrt_absolute_time(); - - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } } - /* 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; + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - /* lazily publish the battery voltage */ - if (pub_hil_battery > 0) { - orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - } else { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - struct rc_channels_s rc_hil; - memset(&rc_hil, 0, sizeof(rc_hil)); - static orb_advert_t rc_pub = 0; - - rc_hil.timestamp = hrt_absolute_time(); - rc_hil.chan_count = 4; - - rc_hil.chan[0].scaled = man.x / 1000.0f; - rc_hil.chan[1].scaled = man.y / 1000.0f; - rc_hil.chan[2].scaled = man.r / 1000.0f; - rc_hil.chan[3].scaled = man.z / 1000.0f; - - struct manual_control_setpoint_s mc; - static orb_advert_t mc_pub = 0; - - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc); - - mc.timestamp = rc_hil.timestamp; - mc.roll = man.x / 1000.0f; - mc.pitch = man.y / 1000.0f; - mc.yaw = man.r / 1000.0f; - mc.throttle = man.z / 1000.0f; - - /* fake RC channels with manual control input from simulator */ - - - if (rc_pub == 0) { - rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); - - } else { - orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); - } - - if (mc_pub == 0) { - mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } } } @@ -799,11 +813,13 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; - /* Set thread name */ + /* 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; fds[0].events = POLLIN; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 199e426892..b97919e9da 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -100,7 +100,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink *_mavlink; @@ -109,37 +108,29 @@ private: void *receive_thread(void *arg); mavlink_status_t status; - struct vehicle_vicon_position_s vicon_position; - struct vehicle_command_s vcmd; - struct offboard_control_setpoint_s offboard_control_sp; - struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; - struct vehicle_attitude_s hil_attitude; - struct vehicle_gps_position_s hil_gps; - struct sensor_combined_s hil_sensors; - struct battery_status_s hil_battery_status; - struct position_setpoint_triplet_s pos_sp_triplet; - orb_advert_t pub_hil_global_pos; - orb_advert_t pub_hil_local_pos; - orb_advert_t pub_hil_attitude; - orb_advert_t pub_hil_gps; - orb_advert_t pub_hil_sensors; - orb_advert_t pub_hil_gyro; - orb_advert_t pub_hil_accel; - orb_advert_t pub_hil_mag; - orb_advert_t pub_hil_baro; - orb_advert_t pub_hil_airspeed; - orb_advert_t pub_hil_battery; - int hil_counter; - int hil_frames; - uint64_t old_timestamp; - orb_advert_t cmd_pub; - orb_advert_t flow_pub; - orb_advert_t offboard_control_sp_pub; - orb_advert_t vicon_position_pub; - orb_advert_t telemetry_status_pub; - int32_t lat0; - int32_t lon0; - float alt0; - + 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; }; From 14ddf1804aa9d698724fda62288b210fd850d2ac Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:48:20 +0400 Subject: [PATCH 57/99] mavlink: code style fixed --- src/modules/mavlink/mavlink_messages.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c22289772c..d4c77d1fd3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -780,6 +780,7 @@ protected: /* set number of valid outputs depending on vehicle type */ unsigned n; + switch (mavlink_system.type) { case MAV_TYPE_QUADROTOR: n = 4; @@ -807,6 +808,7 @@ protected: /* send 0 when disarmed */ out[i] = 0.0f; } + } else { out[i] = -1.0f; } From 190eb6205dc3e610d223878c4b85a8e587fc6323 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 11:45:58 +0400 Subject: [PATCH 58/99] mavlink: OPTICAL_FLOW stream implemented --- src/modules/mavlink/mavlink_messages.cpp | 51 +++++++++++++++++++----- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d4c77d1fd3..7475160d5e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1063,6 +1063,45 @@ protected: }; +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(), @@ -1085,6 +1124,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), + new MavlinkStreamOpticalFlow(), nullptr }; @@ -1143,17 +1183,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_optical_flow(const struct listener *l) -//{ -// struct optical_flow_s flow; -// -// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); -// -// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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 //MavlinkOrbListener::l_nav_cap(const struct listener *l) //{ // From 2ec4ee6fc08f5368a52028de83f420ffeb249698 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 12:33:03 +0400 Subject: [PATCH 59/99] mavlink_receiver: split message handlers to separate methods --- src/modules/mavlink/mavlink_receiver.cpp | 1299 ++++++++++++---------- src/modules/mavlink/mavlink_receiver.h | 11 + 2 files changed, 692 insertions(+), 618 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8a80277386..ef1a747da0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -36,6 +36,7 @@ * MAVLink protocol message receive and dispatch * * @author Lorenz Meier + * @author Anton Babushkin */ /* XXX trim includes */ @@ -120,278 +121,37 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : void MavlinkReceiver::handle_message(mavlink_message_t *msg) { - uint64_t timestamp = hrt_absolute_time(); + switch (msg->msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: + handle_message_command_long(msg); + break; - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - /* command */ - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); + case MAVLINK_MSG_ID_OPTICAL_FLOW: + handle_message_optical_flow(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_SET_MODE: + handle_message_set_mode(msg); + break; - /* terminate other threads and this thread */ - _mavlink->_task_should_exit = true; + case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: + handle_message_vicon_position_estimate(msg); + break; - } else { - struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); + case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: + handle_message_quad_swarm_roll_pitch_yaw_thrust(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; + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status(msg); + break; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; - } else { - /* publish */ - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - } - } - - } else if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - /* 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 = timestamp; - 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); - } - - } else 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); - - 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; - - /* 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); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { - /* vicon */ - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - struct vehicle_vicon_position_s vicon_position; - memset(&vicon_position, 0, sizeof(vicon_position)); - vicon_position.timestamp = timestamp; - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.yaw; - - if (_vicon_position_pub <= 0) { - _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - - } else { - orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - /* offboard control */ - 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) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* switch to a receiving link mode */ - //TODO why do we need this? - //_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); - - /* - * 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 = static_cast(ml_mode); - - offboard_control_sp.timestamp = timestamp; - - 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(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); - } - } - - } else if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { - /* telemetry status */ - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = timestamp; - 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); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - /* manual control */ - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - - rc.timestamp = timestamp; - rc.chan_count = 4; - - 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; - - if (_rc_pub == 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } - } - - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* 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); - - manual.timestamp = timestamp; - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; - - if (_manual_pub == 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } - } + default: + break; } /* @@ -401,400 +161,703 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * in the system mode. Either send a set mode * COMMAND_LONG message or a SET_MODE message */ - if (_mavlink->get_hil_enabled()) { - if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - /* HIL sensors */ - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; - float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; - airspeed.timestamp = timestamp; - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; + default: + break; + } + } +} - if (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); +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); - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } + 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); - /* gyro */ - { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; - 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; + } else { + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); - if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + /* 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; - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } - } - - /* accelerometer */ - { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); - - 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; - - if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } - } - - /* magnetometer */ - { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); - - 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; - - if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - } - - /* baro */ - { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); - - baro.timestamp = timestamp; - baro.pressure = imu.abs_pressure; - baro.altitude = imu.pressure_alt; - baro.temperature = imu.temperature; - - if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } - } - - /* 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); - } - } - - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; - - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - - } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } - - /* increment counters */ - _hil_counter++; - _hil_frames++; - - /* 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; - } - - } else if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - /* HIL GPS */ - mavlink_hil_gps_t gps; - mavlink_msg_hil_gps_decode(msg, &gps); - - struct vehicle_gps_position_s hil_gps; - memset(&hil_gps, 0, sizeof(hil_gps)); - - hil_gps.timestamp_time = timestamp; - hil_gps.time_gps_usec = gps.time_usec; - - 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 - - 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; - - 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); - - hil_gps.timestamp_satellites = timestamp; - hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; - - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + /* check if topic is advertised */ + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + /* publish */ + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } + } + } +} - } else if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - /* HIL state quaternion */ - mavlink_hil_state_quaternion_t hil_state; - mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); +void +MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); - 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; + 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 (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + if (_flow_pub <= 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } +} - /* 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(); +void +MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) +{ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); - hil_attitude.timestamp = timestamp; - memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); - hil_attitude.R_valid = true; + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); - 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; + 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; - 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 (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } +} - } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - } - } +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); - /* global position */ - { - struct vehicle_global_position_s hil_global_pos; - memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); - 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; + 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 (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_vicon_position_pub <= 0) { + _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - } - } + } else { + orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); + } +} - /* 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; - } +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); - 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; + if (mavlink_system.sysid < 4) { + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; + uint8_t ml_mode = 0; + bool ml_armed = false; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + break; - } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); - } - } + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - /* accelerometer */ - { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); + break; - 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; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; - if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + break; - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } - } + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; + 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 (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + ml_armed = false; + } - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = static_cast(ml_mode); + + offboard_control_sp.timestamp = hrt_absolute_time(); + + 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(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + } +} + +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); + } +} + +void +MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) +{ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + /* rc channels */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); + + rc.timestamp = hrt_absolute_time(); + rc.chan_count = 4; + + 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; + + if (_rc_pub == 0) { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + + } else { + orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); + } + } + + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* 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); + + 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; + + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } + } +} + +void +MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + + uint64_t timestamp = hrt_absolute_time(); + + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + 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; + + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + 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; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + 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; + + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } + + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); + + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } + + /* 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); + } + } + + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + } + + /* increment counters */ + _hil_counter++; + _hil_frames++; + + /* 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; + } +} + +void +MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) +{ + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); + + uint64_t timestamp = hrt_absolute_time(); + + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); + + hil_gps.timestamp_time = timestamp; + hil_gps.time_gps_usec = gps.time_usec; + + 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 + + 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; + + 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); + + hil_gps.timestamp_satellites = timestamp; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; + + if (_gps_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + + } else { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } +} + +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); + + uint64_t timestamp = hrt_absolute_time(); + + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + + 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; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* 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); + + } else { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } + + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + + 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; + + if (_global_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + + } else { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } + } + + /* 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; + } + + 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); + + } else { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + 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; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b97919e9da..beaae20587 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -105,6 +105,17 @@ private: Mavlink *_mavlink; 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; From e1742eea7d951aa704665c2b5db4d1c6788aafa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 13:03:17 +0400 Subject: [PATCH 60/99] mavlink: code style fixed --- src/modules/mavlink/mavlink_messages.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7475160d5e..4d83afe825 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1092,12 +1092,12 @@ protected: 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); + 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); } }; From b40d3e1e18bd8032eee97abdd9a4f68d87303f65 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 22:30:43 +0400 Subject: [PATCH 61/99] Change NFILE_DESCRIPTORS back to 32 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 7aa71f1826..dab62df5fd 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=64 +CONFIG_NFILE_DESCRIPTORS=32 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 d42b23ab0b..e560c805a0 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=64 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From 8425b9bef21e310d1cbd29aad65d34e9dd974d55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 22:46:22 +0400 Subject: [PATCH 62/99] Increase NFILE_DESCRIPTORS to 36 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index dab62df5fd..8cb8736404 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 e560c805a0..0fceb2043f 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 From 39b1f7244ce06ff89492ad6843c5fc74a30dfe67 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 8 Mar 2014 11:37:49 +0400 Subject: [PATCH 63/99] mavlink: RC_CHANNELS_RAW added to default streams set --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c10c95dbdb..788fe5732d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1731,6 +1731,7 @@ Mavlink::task_main(int argc, char *argv[]) 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: @@ -1743,6 +1744,7 @@ Mavlink::task_main(int argc, char *argv[]) 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; From b3839afbbc904fe0009610dae57a4626b3306fb6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 11:12:39 +0400 Subject: [PATCH 64/99] mavlink: channel ID allocation fixed --- src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 6 ++- src/modules/mavlink/mavlink_receiver.cpp | 4 +- 3 files changed, 53 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 788fe5732d..4bc1055d1e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,8 +159,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -Mavlink::Mavlink() : +Mavlink::Mavlink(int instance_id) : next(nullptr), + _instance_id(instance_id), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), _mavlink_fd(-1), @@ -179,6 +180,40 @@ Mavlink::Mavlink() : { _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; + + /* set channel according to instance id */ + switch (_instance_id) { + case 0: + _channel = MAVLINK_COMM_0; + break; + case 1: + _channel = MAVLINK_COMM_1; + break; + case 2: + _channel = MAVLINK_COMM_2; + break; + case 3: + _channel = MAVLINK_COMM_3; + break; +#ifdef MAVLINK_COMM_4 + case 4: + _channel = MAVLINK_COMM_4; + break; +#endif +#ifdef MAVLINK_COMM_5 + case 5: + _channel = MAVLINK_COMM_5; + break; +#endif +#ifdef MAVLINK_COMM_6 + case 6: + _channel = MAVLINK_COMM_6; + break; +#endif + default: + errx(1, "instance ID is out of range"); + break; + } } Mavlink::~Mavlink() @@ -226,7 +261,9 @@ Mavlink::instance_count() Mavlink * Mavlink::new_instance() { - Mavlink *inst = new Mavlink(); + int id = Mavlink::instance_count(); + + Mavlink *inst = new Mavlink(id); Mavlink *next = ::_mavlink_instances; /* create the first instance at _head */ @@ -353,6 +390,12 @@ Mavlink::get_uart_fd() return _uart_fd; } +int +Mavlink::get_instance_id() +{ + return _instance_id; +} + mavlink_channel_t Mavlink::get_channel() { @@ -1881,7 +1924,7 @@ int Mavlink::start(int argc, char *argv[]) { // Instantiate thread - char buf[32]; + char buf[24]; sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); task_spawn_cmd(buf, diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b52c12796d..c606da5045 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -105,7 +105,7 @@ public: /** * Constructor */ - Mavlink(); + Mavlink(int instance_id); /** * Destructor, also kills the mavlinks task. @@ -182,6 +182,8 @@ public: MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + int get_instance_id(); + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ @@ -190,6 +192,8 @@ protected: Mavlink *next; private: + int _instance_id; + int _mavlink_fd; bool _task_running; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ef1a747da0..c9fcc4cd84 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -877,8 +877,8 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; /* set thread name */ - char thread_name[18]; - sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); From 0ad38a73231877ec142946cb4c380c74a1633e63 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 15:34:27 +0400 Subject: [PATCH 65/99] mavlink: task stop fixes, WIP --- src/modules/mavlink/mavlink_main.cpp | 67 +++++++++++----------------- src/modules/mavlink/mavlink_main.h | 2 +- 2 files changed, 28 insertions(+), 41 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4bc1055d1e..2dfc67fd4d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,9 +159,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -Mavlink::Mavlink(int instance_id) : +Mavlink::Mavlink() : next(nullptr), - _instance_id(instance_id), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), _mavlink_fd(-1), @@ -181,6 +180,8 @@ Mavlink::Mavlink(int instance_id) : _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; + _instance_id = Mavlink::instance_count(); + /* set channel according to instance id */ switch (_instance_id) { case 0: @@ -214,10 +215,15 @@ Mavlink::Mavlink(int instance_id) : errx(1, "instance ID is out of range"); break; } + + LL_APPEND(_mavlink_instances, this); } Mavlink::~Mavlink() { + warnx("DESTRUCTOR"); + usleep(1000000); + if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; @@ -237,6 +243,7 @@ Mavlink::~Mavlink() } } while (_task_running); } + LL_DELETE(_mavlink_instances, this); } void @@ -258,31 +265,6 @@ Mavlink::instance_count() return inst_index; } -Mavlink * -Mavlink::new_instance() -{ - int id = Mavlink::instance_count(); - - Mavlink *inst = new Mavlink(id); - 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) { @@ -342,13 +324,8 @@ Mavlink::destroy_all_instances() return ERROR; } } - - delete inst_to_del; } - /* reset head */ - ::_mavlink_instances = nullptr; - printf("\n"); warnx("all instances stopped"); return OK; @@ -1659,7 +1636,7 @@ Mavlink::task_main(int argc, char *argv[]) if (err_flag) { usage(); - exit(1); + return ERROR; } if (_datarate == 0) { @@ -1672,7 +1649,8 @@ Mavlink::task_main(int argc, char *argv[]) } if (Mavlink::instance_exists(_device_name, this)) { - errx(1, "mavlink instance for %s already running", _device_name); + warnx("mavlink instance for %s already running", _device_name); + return ERROR; } /* inform about mode */ @@ -1727,7 +1705,8 @@ Mavlink::task_main(int argc, char *argv[]) _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); + warn("could not open %s", _device_name); + return ERROR; } /* create the device node that's used for sending text log messages, etc. */ @@ -1906,18 +1885,23 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_destroy(&_logbuffer); warnx("exiting"); - _task_running = false; - _exit(0); + + return OK; } int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ - Mavlink *instance = Mavlink::new_instance(); + Mavlink *instance = new Mavlink(); /* this will actually only return once MAVLink exits */ - return instance->task_main(argc, argv); + int res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + + return res; } int @@ -2002,13 +1986,14 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - errx(1, "usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("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(); + exit(1); } if (!strcmp(argv[1], "start")) { @@ -2017,6 +2002,7 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); usage(); + exit(1); } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); @@ -2029,6 +2015,7 @@ int mavlink_main(int argc, char *argv[]) } else { usage(); + exit(1); } return 0; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c606da5045..ffc69bc810 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -105,7 +105,7 @@ public: /** * Constructor */ - Mavlink(int instance_id); + Mavlink(); /** * Destructor, also kills the mavlinks task. From 35b8d00e17cc8161e9d2687af3f9313d010c11ea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 16:20:37 +0400 Subject: [PATCH 66/99] mavlink: task_main noreturn attribute removed --- src/modules/mavlink/mavlink_main.cpp | 3 --- src/modules/mavlink/mavlink_main.h | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2dfc67fd4d..4c79c67b4a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -221,9 +221,6 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { - warnx("DESTRUCTOR"); - usleep(1000000); - if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ffc69bc810..ccff6614f8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -309,6 +309,6 @@ private: /** * Main mavlink task. */ - int task_main(int argc, char *argv[]) __attribute__((noreturn)); + int task_main(int argc, char *argv[]); }; From 09f18408fc5835622a955c80304ce6599ef98090 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 17:21:03 +0400 Subject: [PATCH 67/99] mavlink: msg buffer size fixed --- src/modules/mavlink/mavlink_main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ccff6614f8..cc4c896b91 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -209,7 +209,7 @@ private: orb_advert_t _mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; + uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; From 9e41f6af18d3d84413501ce37737d574fd20816d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 19:28:48 +0400 Subject: [PATCH 68/99] mavlink: memory leaks on exit fixed, minor fixes --- src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 17 +++++--- 2 files changed, 50 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4c79c67b4a..5bdf6c2622 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -221,6 +221,8 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { + perf_free(_loop_perf); + if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; @@ -393,14 +395,12 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) 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; - + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (!inst->_task_should_exit) { + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; + } } return OK; @@ -1557,9 +1557,6 @@ Mavlink::task_main(int argc, char *argv[]) warnx("start"); fflush(stdout); - /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&_logbuffer, 5); - int ch; _baudrate = 57600; _datarate = 0; @@ -1706,6 +1703,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&_logbuffer, 5); + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1867,6 +1867,30 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; + /* delete streams */ + MavlinkStream *stream_to_del = nullptr; + MavlinkStream *stream_next = _streams; + + while (stream_next != nullptr) { + stream_to_del = stream_next; + stream_next = stream_to_del->next; + delete stream_to_del; + } + + _streams = nullptr; + + /* delete subscriptions */ + MavlinkOrbSubscription *sub_to_del = nullptr; + MavlinkOrbSubscription *sub_next = _subscriptions; + + while (sub_next != nullptr) { + sub_to_del = sub_next; + sub_next = sub_to_del->next; + delete sub_to_del; + } + + _subscriptions = nullptr; + warnx("waiting for UART receive thread"); /* wait for threads to complete */ @@ -1878,6 +1902,9 @@ Mavlink::task_main(int argc, char *argv[]) /* close UART */ close(_uart_fd); + /* close mavlink logging device */ + close(_mavlink_fd); + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c9fcc4cd84..fa63e06c52 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -83,7 +83,7 @@ __BEGIN_DECLS __END_DECLS -static const float mg2ms2 = 9.8f / 1000.0f; +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), @@ -118,6 +118,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : memset(&hil_local_pos, 0, sizeof(hil_local_pos)); } +MavlinkReceiver::~MavlinkReceiver() +{ +} + void MavlinkReceiver::handle_message(mavlink_message_t *msg) { @@ -828,9 +832,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&accel, 0, sizeof(accel)); 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_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; + accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f; + accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f; accel.x = hil_state.xacc; accel.y = hil_state.yacc; accel.z = hil_state.zacc; @@ -925,7 +929,10 @@ void MavlinkReceiver::print_status() void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - return rcv->receive_thread(NULL); + + rcv->receive_thread(NULL); + + delete rcv; } pthread_t From 1fd7b89d3c7c5f9b5c13198c41a015d6c5899260 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:03:49 +0400 Subject: [PATCH 69/99] mavlink: use "normal" mode for HIL operation, only add HIL_CONTROLS stream when HIL enabled --- src/modules/mavlink/mavlink_main.cpp | 119 +++++---------------------- src/modules/mavlink/mavlink_main.h | 10 +-- 2 files changed, 26 insertions(+), 103 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bdf6c2622..e1c53a5993 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -165,7 +165,7 @@ Mavlink::Mavlink() : _task_should_exit(false), _mavlink_fd(-1), _task_running(false), - _mavlink_hil_enabled(false), + _hil_enabled(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -548,38 +548,17 @@ 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); + /* enable HIL */ + if (hil_enabled && !_hil_enabled) { + _hil_enabled = true; + float rate_mult = _datarate / 1000.0f; + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); } - if (!hil_enabled && _mavlink_hil_enabled) { - _mavlink_hil_enabled = false; -// orb_set_interval(subs.spa_sub, 200); + /* disable HIL */ + if (!hil_enabled && _hil_enabled) { + _hil_enabled = false; + configure_stream("HIL_CONTROLS", 0.0f); } else { ret = ERROR; @@ -1445,10 +1424,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) 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); + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; } else { @@ -1560,9 +1537,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; _datarate = 0; - _channel = MAVLINK_COMM_0; - - _mode = MODE_OFFBOARD; + _mode = MAVLINK_MODE_NORMAL; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -1603,17 +1578,8 @@ Mavlink::task_main(int argc, char *argv[]) // 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; + if (strcmp(optarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; } break; @@ -1649,42 +1615,20 @@ Mavlink::task_main(int argc, char *argv[]) /* inform about mode */ switch (_mode) { - case MODE_CUSTOM: + case MAVLINK_MODE_NORMAL: + warnx("mode: NORMAL"); + break; + + case MAVLINK_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; - } + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; warnx("data rate: %d bytes/s", _datarate); warnx("port: %s, baudrate: %d", _device_name, _baudrate); @@ -1741,7 +1685,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("HEARTBEAT", 1.0f); switch (_mode) { - case MODE_OFFBOARD: + case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); @@ -1753,20 +1697,6 @@ Mavlink::task_main(int argc, char *argv[]) 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; } @@ -1795,12 +1725,7 @@ Mavlink::task_main(int argc, char *argv[]) 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); - } + set_hil_enabled(status->hil_state == HIL_STATE_ON); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cc4c896b91..3a8625c6bf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -145,16 +145,14 @@ public: const char *_device_name; enum MAVLINK_MODE { - MODE_CUSTOM = 0, - MODE_OFFBOARD, - MODE_ONBOARD, - MODE_HIL + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM }; void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _mavlink_hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; }; /** * Handle waypoint related messages. @@ -200,7 +198,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* states */ - bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + bool _hil_enabled; /**< Hardware In the Loop mode */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From 4cee3614c7bc2e960ac52e59014bc4d08b8da11e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:04:23 +0400 Subject: [PATCH 70/99] rc.usb: increase data rate to 10000bytes/s for USB --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0de2d42958..558be42755 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -r 5000 -d /dev/ttyACM0 +mavlink start -r 10000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit From 5f847ff8a3df1ba303129a7a4938305a6901755c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 21:14:54 +0100 Subject: [PATCH 71/99] Remove HIL flag on startup --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index eba18ddb19..8cb8f4dc77 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -392,7 +392,7 @@ then if [ $HIL == yes ] then sleep 1 - set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From a37e539647df2b3cfd335b33e4d186c4d39316b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 22:26:12 +0100 Subject: [PATCH 72/99] Correctly initialize mission count --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bdf6c2622..6675804523 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -178,6 +178,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { _wpm = &_wpm_s; + mission.count = 0; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); From f66b0ad8ac926a79608cf872c8e49ea7ace92288 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:04:30 +0100 Subject: [PATCH 73/99] Added hardware flow control to mavlink app. Auto-disables if nothing can be written to the port --- src/modules/mavlink/mavlink_main.cpp | 46 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 7 +++++ 2 files changed, 53 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6675804523..ed3b265f9e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -104,6 +104,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +static uint64_t last_write_times[6] = {0}; + /* * Internal function to send the bytes through the right serial port */ @@ -149,10 +151,28 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t desired = (sizeof(uint8_t) * length); + + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + + if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0 && + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } + ssize_t ret = write(uart, ch, desired); if (ret != desired) { warn("write err"); + } else { + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } @@ -541,9 +561,35 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -1; } + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); + uart_config.c_cflag |= CRTS_IFLOW; + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + + /* setup output flow control */ + (void)enable_flow_control(true); + return _uart_fd; } +int +Mavlink::enable_flow_control(bool enabled) +{ + struct termios uart_config; + int ret = tcgetattr(_uart_fd, &uart_config); + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + return ret; +} + int Mavlink::set_hil_enabled(bool hil_enabled) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cc4c896b91..2a7bd57cfa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -184,6 +184,13 @@ public: int get_instance_id(); + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ From 023c7069c4a340aa1b7c9f05963a066cee368b2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:36:12 +0100 Subject: [PATCH 74/99] Fix FIONWRITE usage --- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed3b265f9e..9a6e1130e3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -158,13 +158,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length */ int buf_free = 0; - if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); - warnx("DISABLING HARDWARE FLOW CONTROL"); + if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + + if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } } ssize_t ret = write(uart, ch, desired); From 76af0970f5220d3a842c4daa353a45ef51df1082 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:46:02 +0100 Subject: [PATCH 75/99] Increased param rate, fixed wrong usage of MAVLink chan. --- src/modules/mavlink/mavlink_main.cpp | 5 ++--- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6675804523..1044984bfd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -661,7 +661,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, - MAVLINK_COMM_0, + _channel, &tx_msg, name_buf, val_buf, @@ -1561,7 +1561,6 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; _datarate = 0; - _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1776,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(100000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fa63e06c52..c222a3ddf4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -912,7 +912,7 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); } } } From 2d2ecbad00c4f21d0c3ded2faa4f9bdb7adefddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:56:26 +0100 Subject: [PATCH 76/99] Fixed more wrong usages of encoding channel --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1044984bfd..d722bec469 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -856,7 +856,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } @@ -879,7 +879,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = seq; - mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); } else if (seq == 0 && _wpm->size == 0) { @@ -902,7 +902,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_component = compid; wpc.count = mission.count; - mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } @@ -933,7 +933,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t 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_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } @@ -953,7 +953,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u 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_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -979,7 +979,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } @@ -1448,7 +1448,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) statustext.text[i] = '\0'; mavlink_message_t msg; - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_msg_statustext_encode_chan(mavlink_system.sysid, mavlink_system.compid, _channel, &msg, &statustext); mavlink_missionlib_send_message(&msg); return OK; From 7604518db479e7cc9618e19b224c5d5c94013142 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 10:29:39 +0100 Subject: [PATCH 77/99] Fixed HW flow control enable / disable scheme, console output cleanup on start --- src/modules/mavlink/mavlink_main.cpp | 45 ++++++++++++++-------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7e1af88240..4420e04fde 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -160,23 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && + if (buf_free == 0) { + + if (last_write_times[(unsigned)channel] != 0 && hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); - warnx("DISABLING HARDWARE FLOW CONTROL"); + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } + + } else { + + /* apparently there is space left, although we might be + * partially overflooding the buffer already */ + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warn("write err"); - } else { - last_write_times[(unsigned)channel] = hrt_absolute_time(); + // XXX do something here, but change to using FIONWRITE and OS buf size for detection } } @@ -536,7 +543,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* 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); + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); close(_uart_fd); return -1; } @@ -552,7 +559,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* 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); + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); close(_uart_fd); return -1; } @@ -560,7 +567,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR SET CONF %s\n", uart_name); close(_uart_fd); return -1; } @@ -574,7 +581,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); /* setup output flow control */ - (void)enable_flow_control(true); + if (enable_flow_control(true)) { + warnx("ERR FLOW CTRL EN"); + } return _uart_fd; } @@ -1604,10 +1613,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::task_main(int argc, char *argv[]) { - /* inform about start */ - warnx("start"); - fflush(stdout); - int ch; _baudrate = 57600; _datarate = 0; @@ -1736,8 +1741,7 @@ Mavlink::task_main(int argc, char *argv[]) break; } - warnx("data rate: %d bytes/s", _datarate); - warnx("port: %s, baudrate: %d", _device_name, _baudrate); + warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1782,9 +1786,6 @@ Mavlink::task_main(int argc, char *argv[]) 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; From 45a18587fc5e96697da586b9bf51ee65c10e3753 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 17:53:25 +0100 Subject: [PATCH 78/99] Fine tuning for flow control disable to stop firing after change is effective --- src/modules/mavlink/mavlink_main.cpp | 35 +++++++++++++++++----------- src/modules/mavlink/mavlink_main.h | 4 ++++ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4420e04fde..b699b86a5e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -112,44 +112,50 @@ static uint64_t last_write_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { - int uart = -1; + Mavlink *instance; switch (channel) { case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); + instance = Mavlink::get_instance(0); break; case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); + instance = Mavlink::get_instance(1); break; case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); + instance = Mavlink::get_instance(2); break; case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); + instance = Mavlink::get_instance(3); break; #ifdef MAVLINK_COMM_4 case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); + instance = Mavlink::get_instance(4); break; #endif #ifdef MAVLINK_COMM_5 case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); + instance = Mavlink::get_instance(5); break; #endif #ifdef MAVLINK_COMM_6 case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); + instance = Mavlink::get_instance(6); break; #endif } + /* no valid instance, bail */ + if (!instance) + return; + + int uart = instance->get_uart_fd(); + ssize_t desired = (sizeof(uint8_t) * length); /* @@ -158,18 +164,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length */ int buf_free = 0; - if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + if (instance->get_flow_control_enabled() + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } } else { @@ -204,6 +208,7 @@ Mavlink::Mavlink() : _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -600,6 +605,10 @@ Mavlink::enable_flow_control(bool enabled) } ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + if (!ret) { + _flow_control_enabled = enabled; + } + return ret; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2a7bd57cfa..94b2c9dbc4 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -156,6 +156,8 @@ public: bool get_hil_enabled() { return _mavlink_hil_enabled; }; + bool get_flow_control_enabled() { return _flow_control_enabled; } + /** * Handle waypoint related messages. */ @@ -248,6 +250,8 @@ private: char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _flow_control_enabled; + /** * Send one parameter. * From 6b79f533388ab78ddbbb8cc316b38c6e7403e46a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 14 Mar 2014 14:40:54 +0100 Subject: [PATCH 79/99] mavlink stream: do not use getopt as it leads to problems with the global optarg variable --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++++--------------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d722bec469..f1a1f95683 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1538,7 +1538,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) /* copy stream name */ unsigned n = strlen(stream_name) + 1; char *s = new char[n]; - memcpy(s, stream_name, n); + strcpy(s, stream_name); /* set subscription task */ _subscribe_to_stream_rate = rate; @@ -1959,36 +1959,33 @@ Mavlink::stream(int argc, char *argv[]) const char *stream_name = nullptr; int ch; - argc -= 1; - argv += 1; + 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, "r:d:s:")) != EOF) { - switch (ch) { - case 'r': - rate = strtod(optarg, nullptr); + int i = 0; + while (i < argc) { + if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { + rate = strtod(argv[i+1], nullptr); if (rate < 0.0f) { err_flag = true; } - - break; - - case 'd': - device_name = optarg; - break; - - case 's': - stream_name = optarg; - break; - - default: + i++; + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { + device_name = argv[i+1]; + i++; + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) { + stream_name = argv[i+1]; + i++; + } else { err_flag = true; - break; } + + i++; } if (!err_flag && rate >= 0.0 && stream_name != nullptr) { From 717e1bd374e7710ce579e91c45852bbba906eba8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:47:26 +0100 Subject: [PATCH 80/99] Removed stupid sensor counter, replaced it with much more useful timestamps --- .../attitude_estimator_ekf_main.cpp | 10 ++---- .../attitude_estimator_so3_main.cpp | 10 ++---- .../position_estimator_inav_main.c | 16 +++++----- src/modules/sdlog2/sdlog2.c | 31 ++++++++++--------- src/modules/sensors/sensors.cpp | 10 +++--- src/modules/uORB/topics/sensor_combined.h | 17 +++++----- 6 files changed, 43 insertions(+), 51 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 00bd23f838..11ae2a2683 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; @@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -392,9 +390,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.timestamp; } @@ -445,9 +442,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.timestamp; } diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e79726613d..7150218ffc 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_so3_params so3_comp_params; @@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,9 +536,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.timestamp; } @@ -550,9 +547,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.timestamp; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a14354138b..368fa6ee25 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -200,8 +200,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - uint32_t accel_counter = 0; - uint32_t baro_counter = 0; + hrt_abstime accel_timestamp = 0; + hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; @@ -310,8 +310,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter != baro_counter) { - baro_counter = sensor.baro_counter; + if (wait_baro && sensor.baro_timestamp != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { @@ -384,7 +384,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter != accel_counter) { + if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; @@ -408,13 +408,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(corr_acc, 0, sizeof(corr_acc)); } - accel_counter = sensor.accelerometer_counter; + accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } - if (sensor.baro_counter != baro_counter) { + if (sensor.baro_timestamp != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_counter = sensor.baro_counter; + baro_timestamp = sensor.baro_timestamp; baro_updates++; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2514bafeee..ee3ec72166 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -887,11 +887,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - uint16_t gyro_counter = 0; - uint16_t accelerometer_counter = 0; - uint16_t magnetometer_counter = 0; - uint16_t baro_counter = 0; - uint16_t differential_pressure_counter = 0; + hrt_abstime gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; + hrt_abstime differential_pressure_timestamp = 0; /* track changes in distance status */ bool dist_bottom_present = false; @@ -976,28 +976,28 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU = false; bool write_SENS = false; - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; + if (buf.sensor.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; write_IMU = true; } - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; + if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.accelerometer_timestamp; write_IMU = true; } - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; + if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.magnetometer_timestamp; write_IMU = true; } - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; + if (buf.sensor.baro_timestamp != barometer_timestamp) { + barometer_timestamp = buf.sensor.baro_timestamp; write_SENS = true; } - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; + if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { + differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; write_SENS = true; } @@ -1023,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; LOGBUFFER_WRITE_AND_COUNT(SENS); } + } /* --- ATTITUDE --- */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..76dc262f13 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -940,7 +940,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - raw.accelerometer_counter++; + raw.accelerometer_timestamp = accel_report.timestamp; } } @@ -966,7 +966,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_counter++; + raw.timestamp = gyro_report.timestamp; } } @@ -996,7 +996,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - raw.magnetometer_counter++; + raw.magnetometer_timestamp = mag_report.timestamp; } } @@ -1014,7 +1014,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_alt_meter = _barometer.altitude; // Altitude in meters raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - raw.baro_counter++; + raw.baro_timestamp = _barometer.timestamp; } } @@ -1028,7 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; - raw.differential_pressure_counter++; + raw.differential_pressure_timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..92812efd49 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -77,34 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot */ + uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - uint16_t gyro_counter; /**< Number of raw measurments taken */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - + uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ }; /** From 3874bca2084bb88dcd739b309bd4a7929db3b417 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:48:33 +0100 Subject: [PATCH 81/99] mavlink: Only send messages when we have updates for them. --- src/modules/mavlink/mavlink_messages.cpp | 462 +++++++++--------- .../mavlink/mavlink_orb_subscription.cpp | 21 +- .../mavlink/mavlink_orb_subscription.h | 9 + src/modules/mavlink/mavlink_receiver.cpp | 9 +- 4 files changed, 276 insertions(+), 225 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d83afe825..7d388f88da 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -216,8 +216,8 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); + (void)pos_sp_triplet_sub->update(t); uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; @@ -261,22 +261,23 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); + if (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); + 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); + } } }; @@ -284,7 +285,7 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) { } @@ -302,10 +303,10 @@ 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; + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; protected: void subscribe(Mavlink *mavlink) @@ -316,42 +317,43 @@ protected: void send(const hrt_abstime t) { - sensor_sub->update(t); + if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; + 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 (accel_timestamp != sensor->accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor->accelerometer_timestamp; + } + + if (gyro_timestamp != sensor->timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor->timestamp; + } + + if (mag_timestamp != sensor->magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor->magnetometer_timestamp; + } + + if (baro_timestamp != sensor->baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor->baro_timestamp; + } + + 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); } - - 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); } }; @@ -382,12 +384,13 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); + if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } } }; @@ -418,17 +421,18 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); + if (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); + 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); + } } }; @@ -483,23 +487,26 @@ protected: 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); + bool updated = att_sub->update(t); + updated |= pos_sub->update(t); + updated |= armed_sub->update(t); + updated |= act_sub->update(t); + updated |= 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; + if (updated) { - mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos->alt, - -pos->vel_d); + 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); + } } }; @@ -530,19 +537,20 @@ protected: void send(const hrt_abstime t) { - gps_sub->update(t); + if (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); + 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); + } } }; @@ -579,10 +587,11 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); - home_sub->update(t); + bool updated = pos_sub->update(t); + updated |= home_sub->update(t); - mavlink_msg_global_position_int_send(_channel, + if (updated) { + mavlink_msg_global_position_int_send(_channel, pos->timestamp / 1000, pos->lat * 1e7, pos->lon * 1e7, @@ -592,6 +601,7 @@ protected: pos->vel_e * 100.0f, pos->vel_d * 100.0f, _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } } }; @@ -622,16 +632,17 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); + if (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); + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } } }; @@ -662,12 +673,17 @@ protected: 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); + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (home_sub->is_published()) { + 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); + } } }; @@ -713,19 +729,20 @@ protected: void send(const hrt_abstime t) { - act_sub->update(t); + if (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]); + 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]); + } } }; @@ -768,57 +785,60 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); - act_sub->update(t); + bool updated = status_sub->update(t); + updated |= pos_sp_triplet_sub->update(t); + updated |= 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); + if (updated) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; + /* 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); - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + /* set number of valid outputs depending on vehicle type */ + unsigned n; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - default: - n = 8; - break; - } + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - /* scale / assign outputs depending on system type */ - float out[8]; + default: + n = 8; + break; + } - 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; + /* 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 { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.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); + 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); + } } }; @@ -849,14 +869,15 @@ protected: void send(const hrt_abstime t) { - pos_sp_triplet_sub->update(t); + if (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)); + 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)); + } } }; @@ -925,14 +946,15 @@ protected: void send(const hrt_abstime t) { - att_sp_sub->update(t); + if (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); + 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); + } } }; @@ -963,14 +985,15 @@ protected: void send(const hrt_abstime t) { - att_rates_sp_sub->update(t); + if (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); + 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); + } } }; @@ -1001,24 +1024,25 @@ protected: void send(const hrt_abstime t) { - rc_sub->update(t); + if (rc_sub->update(t)) { - const unsigned port_width = 8; + 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); + 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); + } } } }; @@ -1050,15 +1074,16 @@ protected: void send(const hrt_abstime t) { - manual_sub->update(t); + if (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); + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } } }; @@ -1089,15 +1114,16 @@ protected: void send(const hrt_abstime t) { - flow_sub->update(t); + if (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); + 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); + } } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 6279e5366a..9963184683 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,11 +46,15 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : + _fd(orb_subscribe(_topic)), + _published(false), + _topic(topic), + _last_check(0), + next(nullptr) { _data = malloc(topic->o_size); memset(_data, 0, topic->o_size); - _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() @@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t) return false; } + +bool +MavlinkOrbSubscription::is_published() +{ + bool updated; + orb_check(_fd, &updated); + + if (updated) { + _published = true; + } + + return _published; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index eacc270349..42d47e96ed 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -54,12 +54,21 @@ public: ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + + /** + * Check if the topic has been published. + * + * This call will return true if the topic was ever published. + * @param true if the topic has been published at least once. + */ + bool is_published(); void *get_data(); const orb_id_t get_topic(); private: const orb_id_t _topic; int _fd; + bool _published; void *_data; hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c222a3ddf4..2ce86396cf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) 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; @@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) 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.accelerometer_timestamp = timestamp; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) 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.magnetometer_timestamp = timestamp; 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.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = _hil_counter; + hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ if (_sensors_pub > 0) { From df5d702baefae7f51161d170c911cb398f418538 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 17:03:39 +0400 Subject: [PATCH 82/99] mavlink: MavlinkOrbSubscription.update() result fixed --- src/modules/mavlink/mavlink_orb_subscription.cpp | 13 ++++++++----- src/modules/mavlink/mavlink_orb_subscription.h | 13 +++++++------ 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 9963184683..e8f9bb75ba 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -78,12 +78,15 @@ MavlinkOrbSubscription::get_data() bool MavlinkOrbSubscription::update(const hrt_abstime t) { - if (_last_check != t) { - _last_check = t; - bool updated; - orb_check(_fd, &updated); + if (_last_check == t) { + /* already checked right now, return result of the check */ + return _updated; - if (updated) { + } else { + _last_check = t; + orb_check(_fd, &_updated); + + if (_updated) { orb_copy(_topic, _fd, _data); return true; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 42d47e96ed..8529721c03 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -48,7 +48,7 @@ class MavlinkOrbSubscription { public: - MavlinkOrbSubscription *next; + MavlinkOrbSubscription *next; /*< pointer to next subscription in list */ MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); @@ -66,11 +66,12 @@ public: const orb_id_t get_topic(); private: - const orb_id_t _topic; - int _fd; - bool _published; - void *_data; - hrt_abstime _last_check; + const orb_id_t _topic; /*< topic metadata */ + int _fd; /*< subscription handle */ + bool _published; /*< topic was ever published */ + void *_data; /*< pointer to data buffer */ + hrt_abstime _last_check; /*< time of last check */ + bool _updated; /*< updated on last check */ }; From 762e8f52893afee6e8ffb6a728e5bc88518829c3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 17:28:34 +0400 Subject: [PATCH 83/99] mavlink_orb_subscription: minor optimization and comment fix --- src/modules/mavlink/mavlink_orb_subscription.cpp | 4 ++++ src/modules/mavlink/mavlink_orb_subscription.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index e8f9bb75ba..4de7228324 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -98,6 +98,10 @@ MavlinkOrbSubscription::update(const hrt_abstime t) bool MavlinkOrbSubscription::is_published() { + if (_published) { + return true; + } + bool updated; orb_check(_fd, &updated); diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 8529721c03..5c6543e813 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -59,7 +59,7 @@ public: * Check if the topic has been published. * * This call will return true if the topic was ever published. - * @param true if the topic has been published at least once. + * @return true if the topic has been published at least once. */ bool is_published(); void *get_data(); From 15c079921bf2f8256e0fcdd5ab9f31157bc09f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:12:03 +0100 Subject: [PATCH 84/99] Fixed missing parent ioctl call --- src/drivers/gps/gps.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a736cbdf6b..785c8beeb0 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,10 +229,15 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); + case SENSORIOCRESET: + cmd_reset(); + break; + } + + default: + /* give it to parent if no one wants it */ + ret = CDev::ioctl(filp, cmd, arg); break; - } unlock(); From 85b7670b44d0af1141fae73593f68d8469846d8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:15:31 +0100 Subject: [PATCH 85/99] compile fix --- src/drivers/gps/gps.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 785c8beeb0..c72f692d73 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,15 +229,15 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); - break; - } + case SENSORIOCRESET: + cmd_reset(); + break; - default: + default: /* give it to parent if no one wants it */ ret = CDev::ioctl(filp, cmd, arg); break; + } unlock(); From 8818425e584c9670dfa9a823b3dfd95147c087d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 18:25:09 +0400 Subject: [PATCH 86/99] commander: fixed message formatting when disabling sensors publishing in HIL mode --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5d..bf49403d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -350,7 +350,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); From bb7962936e15c594821ae10e4510febfec0d6b9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:37:16 +0100 Subject: [PATCH 87/99] CDev: Fixed printf format specifier --- src/drivers/device/cdev.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index b157b3f181..6e2ebb1f7b 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname) if (ret == OK) break; } else { char name[32]; - snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); if (ret == OK) break; } From 772fce9f82749bde8c14199fcc73cfa2235e760e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:38:55 +0100 Subject: [PATCH 88/99] commander: Improve HIL publication blocking call handling --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5d..131d6a550d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -309,10 +309,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s bool valid_transition = false; int ret = ERROR; - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); valid_transition = true; } else { @@ -347,10 +344,16 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + + if (sensfd < 0) { + warn("failed opening device"); + return 1; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "FAIL"); } closedir(d); From 8383603f7691e3bc66534b58654c4d13547632a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:41:03 +0100 Subject: [PATCH 89/99] commander: Linting: Reduce scope of variable --- src/modules/commander/state_machine_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 131d6a550d..5537c4e801 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -337,10 +337,11 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; - struct dirent *direntry; d = opendir("/dev"); if (d) { + struct dirent *direntry; + while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); From 6db2191a71ed0ea38e1fd18886371f099c14d593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:50:20 +0100 Subject: [PATCH 90/99] commander: Skip devices we do not want to touch in HIL --- .../commander/state_machine_helper.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1e892ed77f..9178bcc5ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -351,6 +351,23 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s return 1; } + /* skip serial ports */ + if (!strncmp("tty", direntry-d_name, 3)) { + continue; + } + /* skip mtd devices */ + if (!strncmp("mtd", direntry-d_name, 3)) { + continue; + } + /* skip ram devices */ + if (!strncmp("ram", direntry-d_name, 3)) { + continue; + } + /* skip mavlink */ + if (!strcmp("mavlink", direntry-d_name)) { + continue; + } + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); @@ -359,8 +376,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s closedir(d); - warnx("directory listing ok (FS mounted and readable)"); - } else { /* failed opening dir */ warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); From 2fe9f65c45ba3aaad549b30e3b34568d07ab2b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:16:18 +0100 Subject: [PATCH 91/99] commander: Fixed compile error and some stupidity in usage of path names --- .../commander/state_machine_helper.cpp | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9178bcc5ab..e6f245cf93 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -341,37 +342,52 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (d) { struct dirent *direntry; + char devname[24]; while ((direntry = readdir(d)) != NULL) { - int sensfd = ::open(direntry->d_name, 0); - - if (sensfd < 0) { - warn("failed opening device"); - return 1; - } - /* skip serial ports */ - if (!strncmp("tty", direntry-d_name, 3)) { + if (!strncmp("tty", direntry->d_name, 3)) { continue; } /* skip mtd devices */ - if (!strncmp("mtd", direntry-d_name, 3)) { + if (!strncmp("mtd", direntry->d_name, 3)) { continue; } /* skip ram devices */ - if (!strncmp("ram", direntry-d_name, 3)) { + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { continue; } /* skip mavlink */ - if (!strcmp("mavlink", direntry-d_name)) { + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); continue; } int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); From 2db7d30400d0240baf4532eeef6cb13403417654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:43:09 +0100 Subject: [PATCH 92/99] Do not work on USB UARTs --- src/modules/mavlink/mavlink_main.cpp | 31 +++++++++++++++++----------- src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 11882f0f40..9c2e0178a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -201,6 +201,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _is_usb_uart(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -1; } - /* - * Setup hardware flow control. If the port has no RTS pin this call will fail, - * which is not an issue, but requires a separate call so we can fail silently. - */ - (void)tcgetattr(_uart_fd, &uart_config); - uart_config.c_cflag |= CRTS_IFLOW; - (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + if (!_is_usb_uart) { + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); + uart_config.c_cflag |= CRTS_IFLOW; + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); - /* setup output flow control */ - if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + /* setup output flow control */ + if (enable_flow_control(true)) { + warnx("ERR FLOW CTRL EN"); + } } return _uart_fd; @@ -596,6 +599,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * int Mavlink::enable_flow_control(bool enabled) { + // We can't do this on USB - skip + if (_is_usb_uart) { + return OK; + } + struct termios uart_config; int ret = tcgetattr(_uart_fd, &uart_config); if (enabled) { @@ -1701,10 +1709,9 @@ Mavlink::task_main(int argc, char *argv[]) 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); + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart); if (_uart_fd < 0) { warn("could not open %s", _device_name); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e6e20eb938..f743c25044 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -208,6 +208,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _is_usb_uart; /**< Port is USB */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From ffd0d1032041601916e2bd2faa395ca0af03659c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:25:22 +0100 Subject: [PATCH 93/99] Fix usage of right time stamps --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 11ae2a2683..10a6cd2c5c 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -393,7 +393,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; @@ -445,7 +445,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 7150218ffc..3653defa66 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -539,7 +539,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,7 +550,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; From 8cb5a12cc702ac238f4dc79bf1f1cee3fdee005f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:23 +0100 Subject: [PATCH 94/99] Remove now unused hil_counter --- src/modules/mavlink/mavlink_receiver.cpp | 3 --- src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2ce86396cf..71ea832fdd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,8 +108,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - - _hil_counter(0), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -647,7 +645,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) } /* increment counters */ - _hil_counter++; _hil_frames++; /* print HIL sensors rate */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index beaae20587..0a5a1b5c7a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -139,7 +139,6 @@ private: 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; From dbd467fe1f3779b45090f556fd05b056f068cd34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:49 +0100 Subject: [PATCH 95/99] sensors: Remove effect-less writing of timestamp - was anyway correctly written by gyro update --- src/modules/sensors/sensors.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 76dc262f13..0ed26b54cb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1592,8 +1592,7 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); - /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ - raw.timestamp = hrt_absolute_time(); + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); From 533e3172dc37aeeb2bc4ccc4ee884239d91079f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:24:31 +0100 Subject: [PATCH 96/99] Make PX4IO driver obey HIL as it should --- src/drivers/px4io/px4io.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8e990aa6f0..82f3ba044a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1959,8 +1962,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; From 9cdb416d3a6ceb1c74b03f3f046f5d904bf7fc45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:32:02 +0100 Subject: [PATCH 97/99] Teach RGB led driver to forward unknown IOCTLs --- src/drivers/rgbled/rgbled.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4f58891ed3..e390140de3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); break; } From 75ad1c4a1350d73315f48a1f4b9bdbee2d20fa9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:35:07 +0100 Subject: [PATCH 98/99] Completely and properly populate battery status message in HIL --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 71ea832fdd..489d2bdcb7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -851,7 +851,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); From 51658ac857390f20d3cc9e2e59a485516c4e0814 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:37:32 +0100 Subject: [PATCH 99/99] Compile fix --- src/drivers/rgbled/rgbled.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index e390140de3..13cbfdfa8b 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -243,7 +243,7 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* see if the parent class can make any use of it */ - ret = CDev::ioctl(filep, cmd, arg); + ret = CDev::ioctl(filp, cmd, arg); break; }