From 581d702478d8671faaf48c32dd7071e06d2b4254 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jan 2014 09:25:39 +0100 Subject: 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 39c8384a7..90f2e2b17 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 -- cgit v1.2.3 From 55eaa38c651be41eee31b86330edbf27869385f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:10:03 +0100 Subject: 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 bb7520a03..63952be6e 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; } -- cgit v1.2.3 From 03c543aba6440c25c5d349791b5a6b33914cb74c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:10:35 +0100 Subject: MAVLink multi-port WIP, not compiling yet --- src/modules/mavlink/mavlink.c | 788 +---------- src/modules/mavlink/mavlink_bridge_header.h | 3 +- src/modules/mavlink/mavlink_hil.h | 52 - src/modules/mavlink/mavlink_main.cpp | 1819 ++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 255 ++++ src/modules/mavlink/mavlink_orb_listener.cpp | 828 ++++++++++++ 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 +- src/modules/mavlink/mavlink_receiver.h | 88 ++ src/modules/mavlink/module.mk | 12 +- src/modules/mavlink/orb_listener.c | 875 ------------- 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 +- 17 files changed, 3199 insertions(+), 2881 deletions(-) delete mode 100644 src/modules/mavlink/mavlink_hil.h create mode 100644 src/modules/mavlink/mavlink_main.cpp create mode 100644 src/modules/mavlink/mavlink_main.h create mode 100644 src/modules/mavlink/mavlink_orb_listener.cpp 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 create mode 100644 src/modules/mavlink/mavlink_receiver.h delete mode 100644 src/modules/mavlink/orb_listener.c 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 4d3c9dd2c..aba8c98da 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 "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 +// #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 /* 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; -} - +/* + * Internal function to send the bytes through the right serial port + */ 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) +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { - int ret = OK; + int uart = -1; - switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* sensor sub triggers scaled IMU */ - orb_set_interval(subs->sensor_sub, min_interval); + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); break; - - case MAVLINK_MSG_ID_HIGHRES_IMU: - /* sensor sub triggers highres IMU */ - orb_set_interval(subs->sensor_sub, min_interval); + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); 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); + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); 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); + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); break; - - case MAVLINK_MSG_ID_MANUAL_CONTROL: - /* manual_control_setpoint triggers this message */ - orb_set_interval(subs->man_control_sp_sub, min_interval); + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); break; - - case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs->debug_key_value, min_interval); + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); break; - - default: - /* not found */ - ret = ERROR; + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); break; } - return ret; -} - - -/**************************************************************************** - * MAVLink text message logger - ****************************************************************************/ - -static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations mavlink_fops = { - .ioctl = mavlink_dev_ioctl -}; - -static int -mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) -{ - static unsigned int total_counter = 0; - - switch (cmd) { - case (int)MAVLINK_IOC_SEND_TEXT_INFO: - case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: - case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; - return OK; - } - - default: - return ENOTTY; - } -} - -#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 0: speed = B0; break; - - case 50: speed = B50; break; - - case 75: speed = B75; break; - - case 110: speed = B110; break; - - case 134: speed = B134; break; - - case 150: speed = B150; break; - - case 200: speed = B200; break; - - case 300: speed = B300; break; - - case 600: speed = B600; break; - - case 1200: speed = B1200; break; - - case 1800: speed = B1800; break; - - case 2400: speed = B2400; break; - - case 4800: speed = B4800; break; - - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - - case 460800: speed = B460800; break; - - case 921600: speed = B921600; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); - return -EINVAL; - } - - /* open uart */ - warnx("UART is %s, baudrate is %d\n", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - *is_usb = false; - - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* USB serial is indicated by /dev/ttyACM0*/ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); - close(uart); - return -1; - } - - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - return uart; -} - -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + } /* @@ -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 149efda60..1fae3ee9d 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_hil.h b/src/modules/mavlink/mavlink_hil.h deleted file mode 100644 index 744ed7d94..000000000 --- a/src/modules/mavlink/mavlink_hil.h +++ /dev/null @@ -1,52 +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 mavlink_hil.h - * Hardware-in-the-loop simulation support. - */ - -#pragma once - -extern bool mavlink_hil_enabled; - -/** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ -extern int set_hil_on_off(bool hil_enabled); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp new file mode 100644 index 000000000..339030a86 --- /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 000000000..8d6f0bd6f --- /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/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp new file mode 100644 index 000000000..3edad0f16 --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -0,0 +1,828 @@ +/**************************************************************************** + * + * 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 + */ + +// XXX trim includes +#include +#include +#include +#include +#include +#include +#include "mavlink_bridge_header.h" +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "mavlink_orb_listener.h" +#include "mavlink_main.h" +//#include "util.h" + + 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); + + +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}, + {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}, +}; + + _n_listeners = sizeof(listeners) / sizeof(listeners[0]); + +} + +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), mavlink_subs.sensor_sub, &raw); + + last_sensor_timestamp = raw.timestamp; + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = raw.accelerometer_counter; + } + + if (gyro_counter != raw.gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = raw.gyro_counter; + } + + if (mag_counter != raw.magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = raw.magnetometer_counter; + } + + if (baro_counter != raw.baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = raw.baro_counter; + } + + if (gcs_link) + mavlink_msg_highres_imu_send(_mavlink->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], + raw.magnetometer_ga[0], + raw.magnetometer_ga[1], raw.magnetometer_ga[2], + raw.baro_pres_mbar, raw.differential_pressure_pa, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); + + sensors_raw_counter++; +} + +void +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->get_mavlink_chan(), + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); + + /* limit VFR message rate to 10Hz */ + hrt_abstime t = hrt_absolute_time(); + if (t >= last_sent_vfr + 100000) { + last_sent_vfr = t; + float groundspeed = sqrtf(global_pos.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->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->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); + } + } + + 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), mavlink_subs.gps_sub, &gps); + + /* GPS COG is 0..2PI in degrees * 1e2 */ + float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; + + /* GPS position */ + mavlink_msg_gps_raw_int_send(_mavlink->get_mavlink_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 && (gps_counter % 50 == 0)) { + mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(), + gps.satellites_visible, + gps.satellite_prn, + gps.satellite_used, + gps.satellite_elevation, + gps.satellite_azimuth, + gps.satellite_snr); + } + + 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); + + /* enable or disable HIL */ + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); +} + +void +MavlinkOrbListener::l_rc_channels(const struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +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); + + if (gcs_link) { + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < (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, + i, + (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, + (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, + rc_raw.rssi); + } + } +} + +void +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->get_mavlink_chan(), + global_pos.timestamp / 1000, + global_pos.lat, + global_pos.lon, + 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, + _wrap_2pi(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); + + 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); +} + +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); + + uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + + 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, + (int32_t)(triplet.current.lat * 1e7d), + (int32_t)(triplet.current.lon * 1e7d), + (int32_t)(triplet.current.altitude * 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), mavlink_subs.spl_sub, &local_sp); + + if (gcs_link) + mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_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), mavlink_subs.spa_sub, &att_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_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), mavlink_subs.rates_setpoint_sub, &rates_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_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 (gcs_link) { + 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], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); + + /* only send in HIL mode and only send first group for HIL */ + if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_base_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_base_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); + + } else { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 500.0f, + (act_outputs.output[1] - 1500.0f) / 500.0f, + (act_outputs.output[2] - 1500.0f) / 500.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 500.0f, + (act_outputs.output[5] - 1500.0f) / 500.0f, + (act_outputs.output[6] - 1500.0f) / 500.0f, + (act_outputs.output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); + } + } + } +} + +void +MavlinkOrbListener::l_actuator_armed(const struct listener *l) +{ + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &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), mavlink_subs.man_control_sp_sub, &man_control); + + if (gcs_link) + mavlink_msg_manual_control_send(_mavlink->get_mavlink_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, 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->get_mavlink_chan(), + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[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->get_mavlink_chan(), + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), + last_sensor_timestamp / 1000, + "ctrl3 ", + 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), mavlink_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, + debug.key, + debug.value); +} + +void +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->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 +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); +} + +void +MavlinkOrbListener::l_airspeed(const struct listener *l) +{ + orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); +} + +void +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->get_mavlink_chan(), + hrt_absolute_time() / 1000, + "turn dist", + 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); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); +} + +static void * +uorb_receive_thread(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 1000; + + /* + * Initialise listener array. + * + * Might want to invoke each listener once to set initial state. + */ + struct pollfd fds[_n_listeners]; + + for (unsigned i = 0; i < _n_listeners; i++) { + fds[i].fd = *listeners[i].subp; + fds[i].events = POLLIN; + + /* Invoke callback to set initial state */ + //listeners[i].callback(&listener[i]); + } + + while (!thread_should_exit) { + + int poll_ret = poll(fds, _n_listeners, timeout); + + /* handle the poll result */ + if (poll_ret == 0) { + /* silent */ + + } else if (poll_ret < 0) { + mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + + } else { + + for (unsigned i = 0; i < _n_listeners; i++) { + if (fds[i].revents & POLLIN) + listeners[i].callback(&listeners[i]); + } + } + } + + return NULL; +} + +pthread_t +uorb_receive_start(void) +{ + /* --- SENSORS RAW VALUE --- */ + mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ + + /* --- ATTITUDE VALUE --- */ + mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ + + /* --- GPS VALUE --- */ + mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ + + /* --- HOME POSITION --- */ + mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ + + /* --- SYSTEM STATE --- */ + status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + + /* --- 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 */ + + /* --- RC CHANNELS VALUE --- */ + rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(rc_sub, 100); /* 10Hz updates */ + + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + + /* --- GLOBAL POS VALUE --- */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ + + /* --- LOCAL POS VALUE --- */ + mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + + /* --- GLOBAL SETPOINT VALUE --- */ + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ + + /* --- LOCAL SETPOINT VALUE --- */ + mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ + + /* --- ATTITUDE SETPOINT VALUE --- */ + mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + + /* --- RATES SETPOINT VALUE --- */ + mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + + /* --- ACTUATOR OUTPUTS --- */ + mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR ARMED VALUE --- */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR CONTROL VALUE --- */ + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ + + /* --- DEBUG VALUE OUTPUT --- */ + mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + + /* --- FLOW SENSOR --- */ + mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + + /* --- AIRSPEED --- */ + mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + + /* start the listener loop */ + pthread_attr_t uorb_attr; + pthread_attr_init(&uorb_attr); + + /* Set stack size, needs less than 2k */ + pthread_attr_setstacksize(&uorb_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + + pthread_attr_destroy(&uorb_attr); + return thread; +} diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h new file mode 100644 index 000000000..32a174fcd --- /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 18ca7a854..000000000 --- a/src/modules/mavlink/mavlink_parameters.c +++ /dev/null @@ -1,230 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 b1e38bcc8..000000000 --- a/src/modules/mavlink/mavlink_parameters.h +++ /dev/null @@ -1,104 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 771989430..6881a2280 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_receiver.h b/src/modules/mavlink/mavlink_receiver.h new file mode 100644 index 000000000..88ae4b110 --- /dev/null +++ b/src/modules/mavlink/mavlink_receiver.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 uORB listener definition + * + * @author Lorenz Meier + */ + +#pragma once + +class Mavlink; + +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 89a097c24..2a005565e 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_listener.c b/src/modules/mavlink/orb_listener.c deleted file mode 100644 index 6e177bc4d..000000000 --- a/src/modules/mavlink/orb_listener.c +++ /dev/null @@ -1,875 +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 orb_listener.c - * Monitors ORB topics and sends update messages as appropriate. - * - * @author Lorenz Meier - */ - -// XXX trim includes -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "util.h" - -extern bool gcs_link; - -struct vehicle_global_position_s global_pos; -struct vehicle_local_position_s local_pos; -struct 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); - -struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; -}; - -uint16_t cm_uint16_from_m_float(float m); - -static void l_sensor_combined(const struct listener *l); -static void l_vehicle_attitude(const struct listener *l); -static void l_vehicle_gps_position(const struct listener *l); -static void l_vehicle_status(const struct listener *l); -static void l_rc_channels(const struct listener *l); -static void l_input_rc(const struct listener *l); -static void l_global_position(const struct listener *l); -static void l_local_position(const struct listener *l); -static void l_global_position_setpoint(const struct listener *l); -static void l_local_position_setpoint(const struct listener *l); -static void l_attitude_setpoint(const struct listener *l); -static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); -static void l_manual_control_setpoint(const struct listener *l); -static void l_vehicle_attitude_controls(const struct listener *l); -static void l_debug_key_value(const struct listener *l); -static void l_optical_flow(const struct listener *l); -static void l_vehicle_rates_setpoint(const struct listener *l); -static void l_home(const struct listener *l); -static void l_airspeed(const struct listener *l); -static void l_nav_cap(const struct listener *l); -static void l_control_mode(const struct listener *l); - -static const struct listener listeners[] = { - {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, - {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, - {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, - {l_vehicle_status, &status_sub, 0}, - {l_rc_channels, &rc_sub, 0}, - {l_input_rc, &mavlink_subs.input_rc_sub, 0}, - {l_global_position, &mavlink_subs.global_pos_sub, 0}, - {l_local_position, &mavlink_subs.local_pos_sub, 0}, - {l_global_position_setpoint, &mavlink_subs.triplet_sub, 0}, - {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, - {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, - {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, - {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, - {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, - {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, - {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, - {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, - {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, - {l_optical_flow, &mavlink_subs.optical_flow, 0}, - {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, - {l_home, &mavlink_subs.home_sub, 0}, - {l_airspeed, &mavlink_subs.airspeed_sub, 0}, - {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, -}; - -static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); - -uint16_t -cm_uint16_from_m_float(float m) -{ - if (m < 0.0f) { - return 0; - - } else if (m > 655.35f) { - return 65535; - } - - return (uint16_t)(m * 100.0f); -} - -void -l_sensor_combined(const struct listener *l) -{ - struct sensor_combined_s raw; - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); - - last_sensor_timestamp = raw.timestamp; - - /* mark individual fields as changed */ - uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; - - if (accel_counter != raw.accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = raw.accelerometer_counter; - } - - if (gyro_counter != raw.gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; - } - - if (mag_counter != raw.magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; - } - - if (baro_counter != raw.baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; - } - - if (gcs_link) - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], - raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], - raw.gyro_rad_s[1], raw.gyro_rad_s[2], - raw.magnetometer_ga[0], - raw.magnetometer_ga[1], raw.magnetometer_ga[2], - raw.baro_pres_mbar, raw.differential_pressure_pa, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); - - sensors_raw_counter++; -} - -void -l_vehicle_attitude(const struct listener *l) -{ - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - - if (gcs_link) { - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); - - /* limit VFR message rate to 10Hz */ - hrt_abstime t = hrt_absolute_time(); - if (t >= last_sent_vfr + 100000) { - last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.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); - } - - /* send quaternion values if it exists */ - if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); - } - } - - attitude_counter++; -} - -void -l_vehicle_gps_position(const struct listener *l) -{ - struct vehicle_gps_position_s gps; - - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); - - /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; - - /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph_m), - cm_uint16_from_m_float(gps.epv_m), - gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from deg to deg * 100 - gps.satellites_visible); - - /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, - gps.satellites_visible, - gps.satellite_prn, - gps.satellite_used, - gps.satellite_elevation, - gps.satellite_azimuth, - gps.satellite_snr); - } - - gps_counter++; -} - -void -l_vehicle_status(const struct listener *l) -{ - /* immediately communicate state changes back to user */ - orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - - /* enable or disable HIL */ - if (v_status.hil_state == HIL_STATE_ON) - set_hil_on_off(true); - else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - -void -l_rc_channels(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - // XXX Add RC channels scaled message here -} - -void -l_input_rc(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); - - if (gcs_link) { - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + 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, - i, - (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, - rc_raw.rssi); - } - } -} - -void -l_global_position(const struct listener *l) -{ - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, - global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, - 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, - _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -} - -void -l_local_position(const struct listener *l) -{ - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - - if (gcs_link) - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); -} - -void -l_global_position_setpoint(const struct listener *l) -{ - struct mission_item_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); - - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - 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_COMM_0, - coordinate_frame, - (int32_t)(triplet.current.lat * 1e7d), - (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), - (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -} - -void -l_local_position_setpoint(const struct listener *l) -{ - struct vehicle_local_position_setpoint_s local_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); - - if (gcs_link) - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); -} - -void -l_attitude_setpoint(const struct listener *l) -{ - struct vehicle_attitude_setpoint_s att_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); - - if (gcs_link) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); -} - -void -l_vehicle_rates_setpoint(const struct listener *l) -{ - struct vehicle_rates_setpoint_s rates_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); - - if (gcs_link) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, - rates_sp.timestamp / 1000, - rates_sp.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); -} - -void -l_actuator_outputs(const struct listener *l) -{ - struct actuator_outputs_s act_outputs; - - orb_id_t ids[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - /* copy actuator data into local buffer */ - orb_copy(ids[l->arg], *l->subp, &act_outputs); - - if (gcs_link) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - l->arg /* port number - needs GCS support */, - /* QGC has port number support already */ - act_outputs.output[0], - act_outputs.output[1], - act_outputs.output[2], - act_outputs.output[3], - act_outputs.output[4], - act_outputs.output[5], - act_outputs.output[6], - act_outputs.output[7]); - - /* only send in HIL mode and only send first group for HIL */ - if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* HIL message as per MAVLink spec */ - - /* scale / assign outputs depending on system type */ - - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 500.0f, - (act_outputs.output[1] - 1500.0f) / 500.0f, - (act_outputs.output[2] - 1500.0f) / 500.0f, - (act_outputs.output[3] - 1000.0f) / 1000.0f, - (act_outputs.output[4] - 1500.0f) / 500.0f, - (act_outputs.output[5] - 1500.0f) / 500.0f, - (act_outputs.output[6] - 1500.0f) / 500.0f, - (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); - } - } - } -} - -void -l_actuator_armed(const struct listener *l) -{ - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); -} - -void -l_manual_control_setpoint(const struct listener *l) -{ - struct manual_control_setpoint_s man_control; - - /* copy manual control data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); - - if (gcs_link) - mavlink_msg_manual_control_send(MAVLINK_COMM_0, - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); -} - -void -l_vehicle_attitude_controls(const struct listener *l) -{ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); - - if (gcs_link) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl0 ", - actuators_0.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl1 ", - actuators_0.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl2 ", - actuators_0.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - "ctrl3 ", - actuators_0.control[3]); - } -} - -void -l_debug_key_value(const struct listener *l) -{ - struct debug_key_value_s debug; - - orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); - - /* Enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - debug.key, - debug.value); -} - -void -l_optical_flow(const struct listener *l) -{ - struct optical_flow_s flow; - - orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); - - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); -} - -void -l_home(const struct listener *l) -{ - 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); -} - -void -l_airspeed(const struct listener *l) -{ - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); -} - -void -l_nav_cap(const struct listener *l) -{ - - orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); - - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, - hrt_absolute_time() / 1000, - "turn dist", - nav_cap.turn_distance); - -} - -void -l_control_mode(const struct listener *l) -{ - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &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); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - -static void * -uorb_receive_thread(void *arg) -{ - /* Set thread name */ - prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; - - /* - * Initialise listener array. - * - * Might want to invoke each listener once to set initial state. - */ - struct pollfd fds[n_listeners]; - - for (unsigned i = 0; i < n_listeners; i++) { - fds[i].fd = *listeners[i].subp; - fds[i].events = POLLIN; - - /* Invoke callback to set initial state */ - //listeners[i].callback(&listener[i]); - } - - while (!thread_should_exit) { - - int poll_ret = poll(fds, n_listeners, timeout); - - /* handle the poll result */ - if (poll_ret == 0) { - /* silent */ - - } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); - - } else { - - for (unsigned i = 0; i < n_listeners; i++) { - if (fds[i].revents & POLLIN) - listeners[i].callback(&listeners[i]); - } - } - } - - return NULL; -} - -pthread_t -uorb_receive_start(void) -{ - /* --- SENSORS RAW VALUE --- */ - mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ - - /* --- ATTITUDE VALUE --- */ - mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ - - /* --- GPS VALUE --- */ - mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ - - /* --- HOME POSITION --- */ - mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); - orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ - - /* --- SYSTEM STATE --- */ - status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - - /* --- 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 */ - - /* --- RC CHANNELS VALUE --- */ - rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ - - /* --- RC RAW VALUE --- */ - mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); - orb_set_interval(mavlink_subs.input_rc_sub, 100); - - /* --- GLOBAL POS VALUE --- */ - mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ - - /* --- LOCAL POS VALUE --- */ - mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ - - /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); - orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ - - /* --- LOCAL SETPOINT VALUE --- */ - mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ - - /* --- ATTITUDE SETPOINT VALUE --- */ - mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ - - /* --- RATES SETPOINT VALUE --- */ - mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ - - /* --- ACTUATOR OUTPUTS --- */ - mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ - - /* --- DEBUG VALUE OUTPUT --- */ - mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ - - /* --- FLOW SENSOR --- */ - mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); - orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ - - /* --- AIRSPEED --- */ - mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ - - /* --- NAVIGATION CAPABILITIES --- */ - mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ - nav_cap.turn_distance = 0.0f; - - /* start the listener loop */ - pthread_attr_t uorb_attr; - pthread_attr_init(&uorb_attr); - - /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); - - pthread_attr_destroy(&uorb_attr); - return thread; -} diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h deleted file mode 100644 index 4d428406a..000000000 --- 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 5e5ee8261..5ca9a085d 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c deleted file mode 100644 index 168666d4e..000000000 --- a/src/modules/mavlink/waypoints.c +++ /dev/null @@ -1,730 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: @author Petri Tanskanen - * @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 f8b58c7d9..532eff7aa 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @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_ */ -- cgit v1.2.3 From ac77fe9c27d7253b01805ff94d3e0f8e21017709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 18:40:02 +0100 Subject: 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 | 94 ++++------ src/modules/mavlink/mavlink_receiver.h | 77 +++++++- 7 files changed, 402 insertions(+), 245 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index aba8c98da..b143e62f0 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 339030a86..1c7986cbb 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 8d6f0bd6f..244af04a6 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; + mavlink_channel_t chan; + +// XXX probably should be in a header... +// extern pthread_t receive_start(int uart); -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + 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 3edad0f16..e2675dfa7 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 32a174fcd..29e081b36 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 6881a2280..ab4074558 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); - -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::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) +{ + +} + +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 88ae4b110..556b6f8ad 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; }; -- cgit v1.2.3 From 9c355d280eb379c72df636c1d47e5b14ae3e3e6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 15:13:14 +0100 Subject: Merged beta into mavlink rework branch --- Documentation/Doxyfile | 2 +- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 58 +- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 85 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 104 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 105 -- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 42 + ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 103 +- .../px4fmu_common/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 | 85 +- 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 | 719 +++++++------- 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 ++++++ src/drivers/frsky_telemetry/frsky_data.h | 51 + src/drivers/frsky_telemetry/frsky_telemetry.c | 266 +++++ src/drivers/frsky_telemetry/module.mk | 41 + src/drivers/gps/gps.cpp | 194 ++-- src/drivers/hil/hil.cpp | 9 +- src/drivers/hmc5883/hmc5883.cpp | 160 +-- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- src/drivers/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 | 221 +++-- 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 +- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- src/lib/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/EulerAngles.hpp | 74 -- 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 | 124 ++- 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/Vector2f.hpp | 79 -- src/lib/mathlib/math/Vector3.cpp | 99 -- src/lib/mathlib/math/Vector3.hpp | 76 -- src/lib/mathlib/math/arm/Matrix.cpp | 40 - src/lib/mathlib/math/arm/Matrix.hpp | 292 ------ src/lib/mathlib/math/arm/Vector.cpp | 40 - src/lib/mathlib/math/arm/Vector.hpp | 236 ----- src/lib/mathlib/math/generic/Matrix.cpp | 40 - src/lib/mathlib/math/generic/Matrix.hpp | 437 --------- src/lib/mathlib/math/generic/Vector.cpp | 40 - src/lib/mathlib/math/generic/Vector.hpp | 245 ----- src/lib/mathlib/mathlib.h | 8 +- src/lib/mathlib/module.mk | 17 - src/lib/version/version.h | 62 ++ src/mainpage.dox | 9 + src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 124 ++- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 20 +- src/modules/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 | 381 ++++---- src/modules/commander/commander_helper.cpp | 44 +- src/modules/commander/commander_helper.h | 7 +- src/modules/commander/commander_params.c | 7 +- src/modules/commander/state_machine_helper.cpp | 142 +-- 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 +- src/modules/fw_att_control/fw_att_control_main.cpp | 10 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 144 ++- .../fw_pos_control_l1/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 +- src/modules/mc_att_control/mc_att_control_main.cpp | 779 +++++++++++++++ src/modules/mc_att_control/mc_att_control_params.c | 53 + src/modules/mc_att_control/module.mk | 41 + src/modules/mc_pos_control/mc_pos_control_main.cpp | 1031 ++++++++++++++++++++ src/modules/mc_pos_control/mc_pos_control_params.c | 58 ++ src/modules/mc_pos_control/module.mk | 41 + src/modules/multirotor_att_control/module.mk | 42 - .../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 -- src/modules/multirotor_pos_control/module.mk | 42 - .../multirotor_pos_control.c | 553 ----------- .../multirotor_pos_control_params.c | 112 --- .../multirotor_pos_control_params.h | 101 -- src/modules/multirotor_pos_control/thrust_pid.c | 189 ---- src/modules/multirotor_pos_control/thrust_pid.h | 76 -- src/modules/navigator/navigator_main.cpp | 1025 +++++++++++-------- src/modules/navigator/navigator_params.c | 7 +- .../position_estimator_inav_main.c | 608 ++++++++---- .../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 | 28 +- 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/sdlog2/sdlog2_version.h | 62 -- src/modules/sensors/module.mk | 2 +- src/modules/sensors/sensor_params.c | 250 ++++- src/modules/sensors/sensors.cpp | 108 +- src/modules/systemlib/board_serial.c | 60 ++ src/modules/systemlib/board_serial.h | 49 + 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 | 114 ++- 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 +- src/modules/uORB/topics/mission_item_triplet.h | 83 -- .../uORB/topics/position_setpoint_triplet.h | 94 ++ src/modules/uORB/topics/vehicle_attitude.h | 1 + .../uORB/topics/vehicle_attitude_setpoint.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 3 +- src/modules/uORB/topics/vehicle_global_position.h | 17 +- src/modules/uORB/topics/vehicle_local_position.h | 5 + src/modules/uORB/topics/vehicle_status.h | 16 +- src/systemcmds/eeprom/24xxxx_mtd.c | 571 ----------- src/systemcmds/eeprom/eeprom.c | 265 ----- src/systemcmds/eeprom/module.mk | 39 - src/systemcmds/hw_ver/hw_ver.c | 73 ++ src/systemcmds/hw_ver/module.mk | 43 + src/systemcmds/mtd/24xxxx_mtd.c | 571 +++++++++++ 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 +- src/systemcmds/tests/test_conv.cpp | 76 ++ 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 + 233 files changed, 11699 insertions(+), 10711 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 create mode 100644 src/drivers/frsky_telemetry/frsky_data.h create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c create mode 100644 src/drivers/frsky_telemetry/module.mk 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/EulerAngles.hpp 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/Vector2f.hpp delete mode 100644 src/lib/mathlib/math/Vector3.cpp delete mode 100644 src/lib/mathlib/math/Vector3.hpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.hpp delete mode 100644 src/lib/mathlib/math/arm/Vector.cpp delete mode 100644 src/lib/mathlib/math/arm/Vector.hpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.cpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.hpp delete mode 100644 src/lib/mathlib/math/generic/Vector.cpp delete mode 100644 src/lib/mathlib/math/generic/Vector.hpp create mode 100644 src/lib/version/version.h create mode 100644 src/mainpage.dox create mode 100644 src/modules/mc_att_control/mc_att_control_main.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_params.c create mode 100644 src/modules/mc_att_control/module.mk create mode 100644 src/modules/mc_pos_control/mc_pos_control_main.cpp create mode 100644 src/modules/mc_pos_control/mc_pos_control_params.c create mode 100644 src/modules/mc_pos_control/module.mk delete mode 100755 src/modules/multirotor_att_control/module.mk 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/module.mk 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 delete mode 100644 src/modules/sdlog2/sdlog2_version.h create mode 100644 src/modules/systemlib/board_serial.c create mode 100644 src/modules/systemlib/board_serial.h create mode 100644 src/modules/systemlib/otp.c create mode 100644 src/modules/systemlib/otp.h delete mode 100644 src/modules/uORB/topics/mission_item_triplet.h create mode 100644 src/modules/uORB/topics/position_setpoint_triplet.h delete mode 100644 src/systemcmds/eeprom/24xxxx_mtd.c delete mode 100644 src/systemcmds/eeprom/eeprom.c delete mode 100644 src/systemcmds/eeprom/module.mk create mode 100644 src/systemcmds/hw_ver/hw_ver.c create mode 100644 src/systemcmds/hw_ver/module.mk create mode 100644 src/systemcmds/mtd/24xxxx_mtd.c 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 create mode 100644 src/systemcmds/tests/test_conv.cpp 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 241702811..b674fbc48 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 40a13b5d1..ebe8a1a1e 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 81d4b5d57..56c74a3b5 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 -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 - 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_YAWRATE_P 0.2 - - param save -fi - +# Maintainers: Simon Wilks # -# 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 +if [ $DO_AUTOCONFIG == yes ] 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 + # + # Default parameters for this platform + # + param set MC_ATT_P 5.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 set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 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 VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# 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 b0f4eda79..a3bcb63eb 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 -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 - 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_YAWRATE_P 0.2 - - param save -fi - +# Maintainers: Anton Babushkin # -# 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 +if [ $DO_AUTOCONFIG == yes ] 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 + # + # Default parameters for this platform + # + param set MC_ATT_P 9.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 set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 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 VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# 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 9b664d63e..000000000 --- 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 000000000..2d497374a --- /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 7b9f41bf6..46da24d35 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 0cc07ad34..e95844891 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 + # - -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 +# Maintainers: Anton Babushkin # -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 344d78422..46da24d35 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 000000000..5f3cec4e0 --- /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 97c2d7c90..0e5bf60d6 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 +# +# Maintainers: ??? # -if param compare SYS_AUTOCONFIG 1 + +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 995d3ba07..1ed923b19 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 a6d2ace96..1ed923b19 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 65f01c974..cbcc6189b 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 + # + # 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 - -# -# 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 +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 0cf6ee39a..000000000 --- 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 000000000..4ebbe9c61 --- /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 41e041654..143310af9 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 -# -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 1 = fixed wing -# -param set MAV_TYPE 1 - +# Skywalker X5 Flying Wing # -# Start and configure PX4IO or FMU interface +# Maintainers: Thomas Gubler , Julian Oes # -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 ] +if [ $DO_AUTOCONFIG == yes ] 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 + # + # 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 -# -# 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 82ff425e6..000000000 --- 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 000000000..e53763278 --- /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 000000000..8d179d1fd --- /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 759c17bb4..000000000 --- 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 2886bcb75..e2cb8833d 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 7054210e2..e0cf92d97 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 # -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 - +# DJI Flame Wheel F330 Quadcopter # -# Start and configure PX4IO or FMU interface +# Maintainers: Anton Babushkin # -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 +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.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 -sh /etc/init.d/rc.mc_interface +set VEHICLE_TYPE mc +set MIXER FMU_quad_x -# -# 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 a1d253191..ced69783d 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 # -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 save -fi - +# DJI Flame Wheel F450 Quadcopter # -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor +# Maintainers: Lorenz Meier # -param set MAV_TYPE 2 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect +if [ $DO_AUTOCONFIG == yes ] 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 + # + # 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 + + # TODO add default MPC parameters 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 VEHICLE_TYPE mc +set MIXER FMU_quad_x -# -# 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 000000000..e1423e008 --- /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 ad455b440..000000000 --- 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 000000000..2e5f6ca4f --- /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 000000000..ddec8f36e --- /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 acd8027fb..000000000 --- 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 000000000..106e0fb54 --- /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 000000000..f0eea339b --- /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 9b90cbdd0..000000000 --- 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 000000000..992a7aeba --- /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 f86f4f85b..000000000 --- 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 000000000..34da2dfef --- /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 40b2ee68b..000000000 --- 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 045e41e52..000000000 --- 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 f02851006..000000000 --- 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 000000000..d354fb06f --- /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 097db28e4..000000000 --- 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 000000000..d25f01dde --- /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 aaf91b316..c9d964f8e 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 dc4be8055..dcf5bbced 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 000000000..96fe32c8a --- /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 6bb2e84ec..000000000 --- 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 bc550ac5a..000000000 --- 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 ecb12e96e..000000000 --- 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 070a4e7e3..badbf92c3 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 67e95215b..000000000 --- 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 ccf2cd47e..0cd8a0e04 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 90f2e2b17..6f4e1f3b5 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 - if param compare SYS_AUTOSTART 1003 + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom + if [ $USE_IO == yes ] + then + set OUTPUT_MODE io + else + set OUTPUT_MODE fmu + fi fi - if param compare SYS_AUTOSTART 1004 + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom + # 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 [ $MODE != custom ] + + if [ $HIL == yes ] then - # Try to get an USB console + 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 - + # - # Upgrade PX4IO firmware + # Start the Commander (needs to be this early for in-air-restarts) # - - 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 + commander start + + # + # Start primary output + # + 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 "[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 "PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + 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 - - 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 - then - sh /etc/init.d/4008_ardrone - set MODE custom - fi + # MAVLink + # + set EXIT_ON_END no - if param compare SYS_AUTOSTART 4009 9 + if [ $HIL == yes ] then - sh /etc/init.d/4009_ardrone_flow - set MODE custom + sleep 1 + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $TTYS1_BUSY == yes ] + then + # 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 + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi 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 + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors - if param compare SYS_AUTOSTART 12001 + if [ $HIL == no ] then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom + echo "[init] Start logging" + sh /etc/init.d/rc.logging + + echo "[init] Start GPS" + gps start 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 + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom + 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 - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi + echo "[init] Vehicle type: MULTICOPTER" - # 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 [ $MIXER == none ] + then + # Set default mixer for multicopter if not defined + set MIXER quad_x + 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 [ $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 + 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 - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom + 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 - 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 + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi + echo "[init] Vehicle type: GENERIC" - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom + # Load mixer and configure outputs + sh /etc/init.d/rc.interface 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 ] + # Start any custom addons + if [ -f $EXTRAS_FILE ] 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 - then - px4io start - else - if fmu mode_serial - then - echo "FMU driver (no PWM) started" - fi - fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - + 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 6aa1d3d46..56482d140 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 000000000..c678ef0f4 --- /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 33f76b415..4d40a6201 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 000000000..33f76b415 --- /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 97410ff47..7ab1454f0 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 000000000..01b5958b7 --- /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 042322aad..e311617f9 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 000000000..0fdb170db --- /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 6d2a9f7bd..000000000 --- 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 fba50aaf8..51be7e1a1 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 f0a9c0c06..ab05d4e3d 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 0f60e88b5..8623c0584 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 e43b9c18e..1dc96b3c3 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 110bcb363..2a734c27e 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 5e45cc936..f73a3ef01 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 6f7166284..02c26b5c0 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 a19ed9d24..7cfca7656 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 269ec238e..71414d62c 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 c3f39addf..1be4877ba 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 8da555211..ef9bb5cad 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 ccd01edf5..9f8c0eeb2 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 51f916f37..88da94b1e 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 000000000..e201ecbb3 --- /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/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h new file mode 100644 index 000000000..a7d9eee53 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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.h + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// 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 000000000..7b08ca69e --- /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/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk new file mode 100644 index 000000000..1632c74f7 --- /dev/null +++ b/src/drivers/frsky_telemetry/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# FrSky telemetry application. +# + +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 fc500a9ec..6b72d560f 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) { + + _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; + + //no time and satellite information simulated + + 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); + } - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + usleep(2e5); - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + } else { - default: - break; - } + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } - unlock(); + 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; + } - if (_Helper->configure(_baudrate) == 0) { unlock(); - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + if (_Helper->configure(_baudrate) == 0) { + unlock(); - 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); + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + 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++; + 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(); - } + /* 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"; + if (!_healthy) { + char *mode_str = "unknown"; - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; - default: - break; + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; } + } - warnx("module found: %s", mode_str); - _healthy = true; + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; } - } - 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 c1d73dd87..0a047f38f 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 d3b99ae66..9b9c11af2 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,31 +915,16 @@ 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}; - warnx("starting mag scale calibration"); + /* 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 }; - /* 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; -// } + warnx("starting mag scale calibration"); /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { @@ -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 e322c6349..a3d3a3933 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 042d9f816..d293f9954 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 3cd6d6720..9251cff7b 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 87788824a..6326cf7fc 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 b878d29bc..c067d363b 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 d918abd57..05bc7a5b3 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 cbdd5acc4..df847a64d 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); + + /* 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"); } + } - // tear down the px4io instance - delete g_dev; + 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 41f93a8ee..dd8abbac5 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 22387a3e2..55f63eef9 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 727c86e02..4f58891ed 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 b286e0007..067d77364 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 b078562c2..614877b18 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 85c63c0fc..0c56494c5 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 b66d1dba5..9584924cc 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 9e7d35f68..2e86c72dc 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 5e2200727..255776765 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 11def2371..3b68a0a4e 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 7a3c42a92..5c0804a39 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 5a56dce65..0f28bccad 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 4fc009da9..d1ebacda1 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 f509f7081..000000000 --- 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 df8970d3a..000000000 --- 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 e733d23bb..000000000 --- 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/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp deleted file mode 100644 index 399eecfa7..000000000 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ /dev/null @@ -1,74 +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 "Vector.hpp" - -namespace math -{ - -class Quaternion; -class Dcm; - -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 - diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3..000000000 --- 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 f19db15ec..ea0cf4ca1 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 02fec4ca6..000000000 --- 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 048a55d33..21d05c7ef 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: - /** - * default ctor + * trivial ctor */ - Quaternion(); + Quaternion() : Vector<4>() {} /** - * ctor from floats + * copy ctor */ - Quaternion(float a, float b, float c, float d); + Quaternion(const Quaternion &q) : Vector<4>(q) {} /** - * ctor from data + * casting from vector */ - Quaternion(const float *data); + Quaternion(const Vector<4> &v) : Vector<4>(v) {} /** - * ctor from Vector + * setting ctor */ - Quaternion(const Vector &v); + Quaternion(const float d[4]) : Vector<4>(d) {} /** - * ctor from EulerAngles + * setting ctor */ - Quaternion(const EulerAngles &euler); + 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 Dcm + * multiplication */ - Quaternion(const Dcm &dcm); + 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]); + } /** - * deep copy ctor + * derivative */ - Quaternion(const Quaternion &right); + 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; + } /** - * dtor + * imaginary part of quaternion */ - virtual ~Quaternion(); + Vector<3> imag(void) { + return Vector<3>(&data[1]); + } /** - * derivative + * set quaternion to rotation defined by euler angles */ - Vector derivative(const Vector &w); + 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]); + } /** - * accessors + * create rotation matrix for the 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); } + 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 35158a396..000000000 --- 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 73de793d5..c7323c215 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 68e741817..000000000 --- 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/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp deleted file mode 100644 index ecd62e81c..000000000 --- a/src/lib/mathlib/math/Vector2f.hpp +++ /dev/null @@ -1,79 +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.hpp - * - * math 3 vector - */ - -#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 - diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600e..000000000 --- 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/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp deleted file mode 100644 index 568d9669a..000000000 --- a/src/lib/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,76 +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.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -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 - diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ /dev/null @@ -1,40 +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 "Matrix.hpp" diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02d..000000000 --- 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.cpp b/src/lib/mathlib/math/arm/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ /dev/null @@ -1,40 +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 "Vector.hpp" diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc15..000000000 --- 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.cpp b/src/lib/mathlib/math/generic/Matrix.cpp deleted file mode 100644 index 21661622a..000000000 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ /dev/null @@ -1,40 +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 "Matrix.hpp" diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447..000000000 --- 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.cpp b/src/lib/mathlib/math/generic/Vector.cpp deleted file mode 100644 index 7ea6496bb..000000000 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ /dev/null @@ -1,40 +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 "Vector.hpp" diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d..000000000 --- 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 40ffb22bc..9e03855c5 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 72bc7db8a..191e2da73 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/lib/version/version.h b/src/lib/version/version.h new file mode 100644 index 000000000..af733aaf0 --- /dev/null +++ b/src/lib/version/version.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 version.h + * + * Tools for system version detection. + * + * @author Anton Babushkin + */ + +#ifndef VERSION_H_ +#define VERSION_H_ + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_GIT STRINGIFY(GIT_VERSION) + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define HW_ARCH "PX4FMU_V1" +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define HW_ARCH "PX4FMU_V2" +#endif + +#endif /* VERSION_H_ */ diff --git a/src/mainpage.dox b/src/mainpage.dox new file mode 100644 index 000000000..7ca410341 --- /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 ecca04dd7..83145ac72 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 a69bde1a6..46ee4b6c8 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 372b2d162..3d20d4d2d 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 a70a14fe4..66ec20b95 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 3cfddf28e..44f47b47c 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 09817d58e..74a141609 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 5eeca5a1a..36b75dd58 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 1e5318121..f579fb52a 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; - - if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + 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; - } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } + if (armed.armed) { + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_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 */ + /* 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"); - status_changed = true; + /* 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; - - transition_result_t res; // store all transitions results here + status.rc_signal_lost = false; - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + transition_result_t res; // store all transitions results here - /* 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) { + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - 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 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) { - } else { - stick_off_counter++; - } + 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 = 0; + stick_off_counter++; } - /* 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 { + stick_off_counter = 0; + } - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + /* 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 { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } - - stick_on_counter = 0; + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - stick_on_counter++; + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else { stick_on_counter = 0; + + } else { + stick_on_counter++; } - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + } else { + stick_on_counter = 0; + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED 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); + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); + } 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); + } - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* fill status according to mode switches */ + check_mode_switches(&sp_man, &status); - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); + /* 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); - } 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); + 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.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; + 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 565b4b66a..21a1c4c2c 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 e9514446c..d0393f45a 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 691d3efcb..d3155f7bf 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 731e0e3ff..c7256583a 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; + break; - case MAIN_STATE_EASY: + 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; + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - case MAIN_STATE_AUTO: + break; - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO: - break; + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; } - if (ret == TRANSITION_CHANGED) { - current_state->main_state = new_main_state; + 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 { + } else { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ + ret = TRANSITION_CHANGED; + break; - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: + 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; - 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; + } + + break; - default: - 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; } - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + 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; + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f3..f04879ff9 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 e213ac17f..e8fecef0d 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 8cc0d77d4..7c80c4b2b 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 108e9896d..f7c0b6148 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 b4dbc36b0..e1c85c261 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 73df3fb9e..888dd0942 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 c94180d68..17b1028f9 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 04caf0bbc..a62b53221 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 9f46b5170..7954d75c2 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 e2675dfa7..44bf77bb0 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 ab4074558..4f763c3c6 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 556b6f8ad..ea57714d2 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 86bfa26f2..bbc9f6e66 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 000000000..245ac024b --- /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/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c new file mode 100644 index 000000000..a170365ee --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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_params.c + * Parameters for multicopter attitude controller. + */ + +#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/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk new file mode 100644 index 000000000..64b876f69 --- /dev/null +++ b/src/modules/mc_att_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = mc_att_control + +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 000000000..d3e39e3a0 --- /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/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c new file mode 100644 index 000000000..9eb56545d --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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_params.c + * Multicopter position controller parameters. + */ + +#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/mc_pos_control/module.mk b/src/modules/mc_pos_control/module.mk new file mode 100644 index 000000000..0b566d7bd --- /dev/null +++ b/src/modules/mc_pos_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build multicopter position controller +# + +MODULE_COMMAND = mc_pos_control + +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_params.c diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk deleted file mode 100755 index 7569e1c7e..000000000 --- a/src/modules/multirotor_att_control/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. -# -############################################################################ - -# -# Build multirotor attitude controller -# - -MODULE_COMMAND = multirotor_att_control - -SRCS = multirotor_att_control_main.c \ - multirotor_attitude_control.c \ - multirotor_rate_control.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 111e9197f..000000000 --- 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 8245aa560..000000000 --- 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 431a435f7..000000000 --- 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 86ac0e4ff..000000000 --- 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 ca7794c59..000000000 --- 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/module.mk b/src/modules/multirotor_pos_control/module.mk deleted file mode 100644 index bc4b48fb4..000000000 --- a/src/modules/multirotor_pos_control/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. -# -############################################################################ - -# -# Build multirotor position control -# - -MODULE_COMMAND = multirotor_pos_control - -SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c \ - thrust_pid.c 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 2ca650420..000000000 --- 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 b7041e4d5..000000000 --- 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 fc658dadb..000000000 --- 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 b985630ae..000000000 --- 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 5e169c1ba..000000000 --- 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 63952be6e..9c5e62be7 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_MISSION: + 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); - } - break; + } else { + dispatch(EVENT_LOITER_REQUESTED); + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + 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; + } - } 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); + /* 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); + } + } } } + + } else { + /* 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"); } - } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - get_loiter_item(&_mission_item_triplet.current); + if (_rtl_state == RTL_STATE_LAND) { + /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ + _rtl_state = RTL_STATE_DESCEND; + } + } - publish_mission_item_triplet(); + _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; + + 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; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - - _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; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + float climb_alt = _home_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; + } + 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; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _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; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _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; + } + 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; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - float descend_alt = _home_pos.altitude + _parameters.land_alt; - - _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; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _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 + _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; + } + 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; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _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; - } + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _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; + } + 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_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; } - publish_mission_item_triplet(); + 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; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; - 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); + 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; + + /* 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; - 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; + 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 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; + 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_termination_enabled = true; 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; + default: 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 8df47fc3b..af1d9d7d5 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 5bf0fba30..af04bb0bc 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_check(armed_sub, &updated); + + if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - } - /* vehicle attitude */ - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; + /* 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 (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; - - } 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; - } + 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(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 */ + 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) { + 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"); + } + + } 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_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 (!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_xy_inited) { + 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 */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; + 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) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + 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 { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + 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 (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_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_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_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 (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 4f9ddd009..e1bbd75a6 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 61570aea7..e2be079d3 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 81566eb2a..2f5908ac5 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 541eed0e1..5e2c92bf4 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 4d306d6d0..60eda2319 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 e55ef784a..2e79f0ac6 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 745bd5705..d4c25911e 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) { + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); - phase = !phase; - usleep(300000); + 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 dea04a663..393e0560e 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 bfc0337f6..2c437d2c0 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 83bd3026e..ff2e4af6e 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 11ccd7356..495447740 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 6833ec43f..9bac2958e 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 (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; + } - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; + /* 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] = ""; - - int fd = 0; - - /* 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); + char log_file_name[16] = ""; + char log_file_path[48] = ""; + + 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); + + } 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); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); - return fd; - } + if (fd < 0) { + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); - 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; + } 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(); + + if (log_fd < 0) + return; - int log_fd = open_logfile(); + 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); - } + gps_time = 0; + + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); + 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 3afaaa2ad..98736dd21 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/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h deleted file mode 100644 index c6a9ba638..000000000 --- a/src/modules/sdlog2/sdlog2_version.h +++ /dev/null @@ -1,62 +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 sdlog2_version.h - * - * Tools for system version detection. - * - * @author Anton Babushkin - */ - -#ifndef SDLOG2_VERSION_H_ -#define SDLOG2_VERSION_H_ - -/* - GIT_VERSION is defined at build time via a Makefile call to the - git command line. - */ -#define FREEZE_STR(s) #s -#define STRINGIFY(s) FREEZE_STR(s) -#define FW_GIT STRINGIFY(GIT_VERSION) - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 -#define HW_ARCH "PX4FMU_V1" -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 -#define HW_ARCH "PX4FMU_V2" -#endif - -#endif /* SDLOG2_VERSION_H_ */ diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index ebbc580e1..aa538fd6b 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 763723554..30659fd3a 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 9baf1a6af..ea864390d 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 = hrt_absolute_time(); + } + _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); + + } 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/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c new file mode 100644 index 000000000..ad8c2bf83 --- /dev/null +++ b/src/modules/systemlib/board_serial.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#include "otp.h" +#include "board_config.h" +#include "board_serial.h" + +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)); + + + /* 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/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h new file mode 100644 index 000000000..b14bb4376 --- /dev/null +++ b/src/modules/systemlib/board_serial.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#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 8aca6a25d..49403c98b 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 843cda722..3953b757d 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 000000000..695574fdc --- /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 000000000..f10e129d8 --- /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 398657dd7..2d773fd25 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 77c952f52..6a4e9392a 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 eca228464..e8b1aac4f 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 cac3dc82a..190b315f1 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) { - - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; - - 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]; - } + progress = diff * 10000 / RAMP_TIME_US; + + for (unsigned i=0; i 0) { - 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]; + /* 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; + } } 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 79a820c06..4c84c1f25 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 c40d0d4e5..d473dff3f 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 3e2fee84e..08d11abae 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/mission_item_triplet.h deleted file mode 100644 index b35eae607..000000000 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @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 mission_item_triplet.h - * Definition of the global WGS84 position setpoint uORB topic. - */ - -#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ -#define TOPIC_MISSION_ITEM_TRIPLET_H_ - -#include -#include -#include "../uORB.h" - -#include "mission.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * 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 -{ - 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; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); - -#endif diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h new file mode 100644 index 000000000..4b57833b6 --- /dev/null +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @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 mission_item_triplet.h + * Definition of the global WGS84 position setpoint uORB topic. + */ + +#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ +#define TOPIC_MISSION_ITEM_TRIPLET_H_ + +#include +#include +#include "../uORB.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 position_setpoint_triplet_s +{ + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +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 4380a5ee7..e5a35ff9b 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 1a245132a..7596f944f 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 26dcbd985..5aecac898 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 143734e37..ae771ca00 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 427153782..d567f2e02 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 1a9dec5f5..a5988d3ba 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/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c deleted file mode 100644 index e34be44e3..000000000 --- a/src/systemcmds/eeprom/24xxxx_mtd.c +++ /dev/null @@ -1,571 +0,0 @@ -/************************************************************************************ - * Driver for 24xxxx-style I2C EEPROMs. - * - * Adapted from: - * - * drivers/mtd/at24xx.c - * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) - * - * Copyright (C) 2011 Li Zhuoyi. All rights reserved. - * Author: Li Zhuoyi - * History: 0.1 2011-08-20 initial version - * - * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn - * - * Derived from drivers/mtd/m25px.c - * - * Copyright (C) 2009-2011 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. - * - ************************************************************************************/ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "systemlib/perf_counter.h" - -/************************************************************************************ - * Pre-processor Definitions - ************************************************************************************/ - -/* - * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be - * omitted in order to avoid building the AT24XX driver as well. - */ - -/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ - -#ifndef CONFIG_AT24XX_SIZE -# warning "Assuming AT24 size 64" -# define CONFIG_AT24XX_SIZE 64 -#endif -#ifndef CONFIG_AT24XX_ADDR -# warning "Assuming AT24 address of 0x50" -# define CONFIG_AT24XX_ADDR 0x50 -#endif - -/* Get the part configuration based on the size configuration */ - -#if CONFIG_AT24XX_SIZE == 32 -# define AT24XX_NPAGES 128 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 48 -# define AT24XX_NPAGES 192 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 64 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 32 -#elif CONFIG_AT24XX_SIZE == 128 -# define AT24XX_NPAGES 256 -# define AT24XX_PAGESIZE 64 -#elif CONFIG_AT24XX_SIZE == 256 -# define AT24XX_NPAGES 512 -# define AT24XX_PAGESIZE 64 -#endif - -/* For applications where a file system is used on the AT24, the tiny page sizes - * will result in very inefficient FLASH usage. In such cases, it is better if - * blocks are comprised of "clusters" of pages so that the file system block - * size is, say, 256 or 512 bytes. In any event, the block size *must* be an - * even multiple of the pages. - */ - -#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE -# warning "Assuming driver block size is the same as the FLASH page size" -# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE -#endif - -/* The AT24 does not respond on the bus during write cycles, so we depend on a long - * timeout to detect problems. The max program time is typically ~5ms. - */ -#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS -# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 -#endif - -/************************************************************************************ - * Private Types - ************************************************************************************/ - -/* This type represents the state of the MTD device. The struct mtd_dev_s - * must appear at the beginning of the definition so that you can freely - * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. - */ - -struct at24c_dev_s { - struct mtd_dev_s mtd; /* MTD interface */ - FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ - uint8_t addr; /* I2C address */ - uint16_t pagesize; /* 32, 63 */ - uint16_t npages; /* 128, 256, 512, 1024 */ - - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; -}; - -/************************************************************************************ - * Private Function Prototypes - ************************************************************************************/ - -/* MTD driver methods */ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buf); -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR const uint8_t *buf); -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); - -void at24c_test(void); - -/************************************************************************************ - * Private Data - ************************************************************************************/ - -/* At present, only a single AT24 part is supported. In this case, a statically - * allocated state structure may be used. - */ - -static struct at24c_dev_s g_at24c; - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -static int at24c_eraseall(FAR struct at24c_dev_s *priv) -{ - int startblock = 0; - uint8_t buf[AT24XX_PAGESIZE + 2]; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - memset(&buf[2], 0xff, priv->pagesize); - - for (startblock = 0; startblock < priv->npages; startblock++) { - uint16_t offset = startblock * priv->pagesize; - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - - while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { - fvdbg("erase stall\n"); - usleep(10000); - } - } - - return OK; -} - -/************************************************************************************ - * Name: at24c_erase - ************************************************************************************/ - -static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) -{ - /* EEprom need not erase */ - - return (int)nblocks; -} - -/************************************************************************************ - * Name: at24c_test - ************************************************************************************/ - -void at24c_test(void) -{ - uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; - unsigned count = 0; - unsigned errors = 0; - - for (count = 0; count < 10000; count++) { - ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); - if (result == ERROR) { - if (errors++ > 2) { - vdbg("too many errors\n"); - return; - } - } else if (result != 1) { - vdbg("unexpected %u\n", result); - } - if ((count % 100) == 0) - vdbg("test %u errors %u\n", count, errors); - } -} - -/************************************************************************************ - * Name: at24c_bread - ************************************************************************************/ - -static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, - size_t nblocks, FAR uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t addr[2]; - int ret; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addr[0], - .length = sizeof(addr), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - addr[1] = offset & 0xff; - addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; - - for (;;) { - - perf_begin(priv->perf_reads); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret >= 0) - break; - - fvdbg("read stall"); - usleep(1000); - - /* We should normally only be here on the first read after - * a write. - * - * XXX maybe do special first-read handling with optional - * bus reset as well? - */ - perf_count(priv->perf_read_retries); - - if (--tries == 0) { - perf_count(priv->perf_read_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_bwrite - * - * Operates on MTD block's and translates to FLASH pages - * - ************************************************************************************/ - -static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, - FAR const uint8_t *buffer) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - size_t blocksleft; - uint8_t buf[AT24XX_PAGESIZE + 2]; - int ret; - - struct i2c_msg_s msgv[1] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); - nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#endif - blocksleft = nblocks; - - if (startblock >= priv->npages) { - return 0; - } - - if (startblock + nblocks > priv->npages) { - nblocks = priv->npages - startblock; - } - - fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); - - while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; - unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - - buf[1] = offset & 0xff; - buf[0] = (offset >> 8) & 0xff; - memcpy(&buf[2], buffer, priv->pagesize); - - for (;;) { - - perf_begin(priv->perf_writes); - ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); - - if (ret >= 0) - break; - - fvdbg("write stall"); - usleep(1000); - - /* We expect to see a number of retries per write cycle as we - * poll for write completion. - */ - if (--tries == 0) { - perf_count(priv->perf_write_errors); - return ERROR; - } - } - - startblock++; - buffer += priv->pagesize; - } - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); -#else - return nblocks; -#endif -} - -/************************************************************************************ - * Name: at24c_ioctl - ************************************************************************************/ - -static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) -{ - FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; - int ret = -EINVAL; /* Assume good command with bad parameters */ - - fvdbg("cmd: %d \n", cmd); - - switch (cmd) { - case MTDIOC_GEOMETRY: { - FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); - - if (geo) { - /* Populate the geometry structure with information need to know - * the capacity and how to access the device. - * - * NOTE: that the device is treated as though it where just an array - * of fixed size blocks. That is most likely not true, but the client - * will expect the device logic to do whatever is necessary to make it - * appear so. - * - * blocksize: - * May be user defined. The block size for the at24XX devices may be - * larger than the page size in order to better support file systems. - * The read and write functions translate BLOCKS to pages for the - * small flash devices - * erasesize: - * It has to be at least as big as the blocksize, bigger serves no - * purpose. - * neraseblocks - * Note that the device size is in kilobits and must be scaled by - * 1024 / 8 - */ - -#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; -#else - geo->blocksize = priv->pagesize; - geo->erasesize = priv->pagesize; - geo->neraseblocks = priv->npages; -#endif - ret = OK; - - fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", - geo->blocksize, geo->erasesize, geo->neraseblocks); - } - } - break; - - case MTDIOC_BULKERASE: - ret = at24c_eraseall(priv); - break; - - case MTDIOC_XIPBASE: - default: - ret = -ENOTTY; /* Bad command */ - break; - } - - return ret; -} - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: at24c_initialize - * - * Description: - * Create an initialize MTD device instance. MTD devices are not registered - * in the file system, but are created as instances that can be bound to - * other functions (such as a block or character driver front end). - * - ************************************************************************************/ - -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { - FAR struct at24c_dev_s *priv; - - fvdbg("dev: %p\n", dev); - - /* Allocate a state structure (we allocate the structure instead of using - * a fixed, static allocation so that we can handle multiple FLASH devices. - * The current implementation would handle only one FLASH part per I2C - * device (only because of the SPIDEV_FLASH definition) and so would have - * to be extended to handle multiple FLASH parts on the same I2C bus. - */ - - priv = &g_at24c; - - if (priv) { - /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; - priv->pagesize = AT24XX_PAGESIZE; - priv->npages = AT24XX_NPAGES; - - priv->mtd.erase = at24c_erase; - priv->mtd.bread = at24c_bread; - priv->mtd.bwrite = at24c_bwrite; - priv->mtd.ioctl = at24c_ioctl; - priv->dev = dev; - - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); - } - - /* attempt to read to validate device is present */ - unsigned char buf[5]; - uint8_t addrbuf[2] = {0, 0}; - - struct i2c_msg_s msgv[2] = { - { - .addr = priv->addr, - .flags = 0, - .buffer = &addrbuf[0], - .length = sizeof(addrbuf), - }, - { - .addr = priv->addr, - .flags = I2C_M_READ, - .buffer = &buf[0], - .length = sizeof(buf), - } - }; - - perf_begin(priv->perf_reads); - int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); - - if (ret < 0) { - return NULL; - } - - /* Return the implementation-specific state structure as the MTD device */ - - fvdbg("Return %p\n", priv); - return (FAR struct mtd_dev_s *)priv; -} - -/* - * XXX: debug hackery - */ -int at24c_nuke(void) -{ - return at24c_eraseall(&g_at24c); -} diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 2aed80e01..000000000 --- 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/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk deleted file mode 100644 index 07f3945be..000000000 --- a/src/systemcmds/eeprom/module.mk +++ /dev/null @@ -1,39 +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. -# -############################################################################ - -# -# EEPROM file system driver -# - -MODULE_COMMAND = eeprom -SRCS = 24xxxx_mtd.c eeprom.c diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c new file mode 100644 index 000000000..4b84523cc --- /dev/null +++ b/src/systemcmds/hw_ver/hw_ver.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 hw_ver.c + * + * Show and test hardware version. + */ + +#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 000000000..3cc08b6a1 --- /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/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c new file mode 100644 index 000000000..e34be44e3 --- /dev/null +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -0,0 +1,571 @@ +/************************************************************************************ + * Driver for 24xxxx-style I2C EEPROMs. + * + * Adapted from: + * + * drivers/mtd/at24xx.c + * Driver for I2C-based at24cxx EEPROM(at24c32,at24c64,at24c128,at24c256) + * + * Copyright (C) 2011 Li Zhuoyi. All rights reserved. + * Author: Li Zhuoyi + * History: 0.1 2011-08-20 initial version + * + * 2011-11-1 Added support for larger MTD block sizes: Hal Glenn + * + * Derived from drivers/mtd/m25px.c + * + * Copyright (C) 2009-2011 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "systemlib/perf_counter.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* + * Configuration is as for the AT24 driver, but the CONFIG_MTD_AT24XX define should be + * omitted in order to avoid building the AT24XX driver as well. + */ + +/* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ + +#ifndef CONFIG_AT24XX_SIZE +# warning "Assuming AT24 size 64" +# define CONFIG_AT24XX_SIZE 64 +#endif +#ifndef CONFIG_AT24XX_ADDR +# warning "Assuming AT24 address of 0x50" +# define CONFIG_AT24XX_ADDR 0x50 +#endif + +/* Get the part configuration based on the size configuration */ + +#if CONFIG_AT24XX_SIZE == 32 +# define AT24XX_NPAGES 128 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 48 +# define AT24XX_NPAGES 192 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 64 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 32 +#elif CONFIG_AT24XX_SIZE == 128 +# define AT24XX_NPAGES 256 +# define AT24XX_PAGESIZE 64 +#elif CONFIG_AT24XX_SIZE == 256 +# define AT24XX_NPAGES 512 +# define AT24XX_PAGESIZE 64 +#endif + +/* For applications where a file system is used on the AT24, the tiny page sizes + * will result in very inefficient FLASH usage. In such cases, it is better if + * blocks are comprised of "clusters" of pages so that the file system block + * size is, say, 256 or 512 bytes. In any event, the block size *must* be an + * even multiple of the pages. + */ + +#ifndef CONFIG_AT24XX_MTD_BLOCKSIZE +# warning "Assuming driver block size is the same as the FLASH page size" +# define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE +#endif + +/* The AT24 does not respond on the bus during write cycles, so we depend on a long + * timeout to detect problems. The max program time is typically ~5ms. + */ +#ifndef CONFIG_AT24XX_WRITE_TIMEOUT_MS +# define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/* This type represents the state of the MTD device. The struct mtd_dev_s + * must appear at the beginning of the definition so that you can freely + * cast between pointers to struct mtd_dev_s and struct at24c_dev_s. + */ + +struct at24c_dev_s { + struct mtd_dev_s mtd; /* MTD interface */ + FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */ + uint8_t addr; /* I2C address */ + uint16_t pagesize; /* 32, 63 */ + uint16_t npages; /* 128, 256, 512, 1024 */ + + perf_counter_t perf_reads; + perf_counter_t perf_writes; + perf_counter_t perf_resets; + perf_counter_t perf_read_retries; + perf_counter_t perf_read_errors; + perf_counter_t perf_write_errors; +}; + +/************************************************************************************ + * Private Function Prototypes + ************************************************************************************/ + +/* MTD driver methods */ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buf); +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR const uint8_t *buf); +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); + +void at24c_test(void); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/* At present, only a single AT24 part is supported. In this case, a statically + * allocated state structure may be used. + */ + +static struct at24c_dev_s g_at24c; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +static int at24c_eraseall(FAR struct at24c_dev_s *priv) +{ + int startblock = 0; + uint8_t buf[AT24XX_PAGESIZE + 2]; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + memset(&buf[2], 0xff, priv->pagesize); + + for (startblock = 0; startblock < priv->npages; startblock++) { + uint16_t offset = startblock * priv->pagesize; + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + + while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) { + fvdbg("erase stall\n"); + usleep(10000); + } + } + + return OK; +} + +/************************************************************************************ + * Name: at24c_erase + ************************************************************************************/ + +static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks) +{ + /* EEprom need not erase */ + + return (int)nblocks; +} + +/************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + +/************************************************************************************ + * Name: at24c_bread + ************************************************************************************/ + +static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, + size_t nblocks, FAR uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t addr[2]; + int ret; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addr[0], + .length = sizeof(addr), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = 0, + .length = priv->pagesize, + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + addr[1] = offset & 0xff; + addr[0] = (offset >> 8) & 0xff; + msgv[1].buffer = buffer; + + for (;;) { + + perf_begin(priv->perf_reads); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret >= 0) + break; + + fvdbg("read stall"); + usleep(1000); + + /* We should normally only be here on the first read after + * a write. + * + * XXX maybe do special first-read handling with optional + * bus reset as well? + */ + perf_count(priv->perf_read_retries); + + if (--tries == 0) { + perf_count(priv->perf_read_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_bwrite + * + * Operates on MTD block's and translates to FLASH pages + * + ************************************************************************************/ + +static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, + FAR const uint8_t *buffer) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + size_t blocksleft; + uint8_t buf[AT24XX_PAGESIZE + 2]; + int ret; + + struct i2c_msg_s msgv[1] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#endif + blocksleft = nblocks; + + if (startblock >= priv->npages) { + return 0; + } + + if (startblock + nblocks > priv->npages) { + nblocks = priv->npages - startblock; + } + + fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + while (blocksleft-- > 0) { + uint16_t offset = startblock * priv->pagesize; + unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; + + buf[1] = offset & 0xff; + buf[0] = (offset >> 8) & 0xff; + memcpy(&buf[2], buffer, priv->pagesize); + + for (;;) { + + perf_begin(priv->perf_writes); + ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); + perf_end(priv->perf_writes); + + if (ret >= 0) + break; + + fvdbg("write stall"); + usleep(1000); + + /* We expect to see a number of retries per write cycle as we + * poll for write completion. + */ + if (--tries == 0) { + perf_count(priv->perf_write_errors); + return ERROR; + } + } + + startblock++; + buffer += priv->pagesize; + } + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); +#else + return nblocks; +#endif +} + +/************************************************************************************ + * Name: at24c_ioctl + ************************************************************************************/ + +static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) +{ + FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + fvdbg("cmd: %d \n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to know + * the capacity and how to access the device. + * + * NOTE: that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the client + * will expect the device logic to do whatever is necessary to make it + * appear so. + * + * blocksize: + * May be user defined. The block size for the at24XX devices may be + * larger than the page size in order to better support file systems. + * The read and write functions translate BLOCKS to pages for the + * small flash devices + * erasesize: + * It has to be at least as big as the blocksize, bigger serves no + * purpose. + * neraseblocks + * Note that the device size is in kilobits and must be scaled by + * 1024 / 8 + */ + +#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE + geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; +#else + geo->blocksize = priv->pagesize; + geo->erasesize = priv->pagesize; + geo->neraseblocks = priv->npages; +#endif + ret = OK; + + fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case MTDIOC_BULKERASE: + ret = at24c_eraseall(priv); + break; + + case MTDIOC_XIPBASE: + default: + ret = -ENOTTY; /* Bad command */ + break; + } + + return ret; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: at24c_initialize + * + * Description: + * Create an initialize MTD device instance. MTD devices are not registered + * in the file system, but are created as instances that can be bound to + * other functions (such as a block or character driver front end). + * + ************************************************************************************/ + +FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { + FAR struct at24c_dev_s *priv; + + fvdbg("dev: %p\n", dev); + + /* Allocate a state structure (we allocate the structure instead of using + * a fixed, static allocation so that we can handle multiple FLASH devices. + * The current implementation would handle only one FLASH part per I2C + * device (only because of the SPIDEV_FLASH definition) and so would have + * to be extended to handle multiple FLASH parts on the same I2C bus. + */ + + priv = &g_at24c; + + if (priv) { + /* Initialize the allocated structure */ + + priv->addr = CONFIG_AT24XX_ADDR; + priv->pagesize = AT24XX_PAGESIZE; + priv->npages = AT24XX_NPAGES; + + priv->mtd.erase = at24c_erase; + priv->mtd.bread = at24c_bread; + priv->mtd.bwrite = at24c_bwrite; + priv->mtd.ioctl = at24c_ioctl; + priv->dev = dev; + + priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); + priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); + priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); + priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); + priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); + priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + } + + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + + /* Return the implementation-specific state structure as the MTD device */ + + fvdbg("Return %p\n", priv); + return (FAR struct mtd_dev_s *)priv; +} + +/* + * XXX: debug hackery + */ +int at24c_nuke(void) +{ + return at24c_eraseall(&g_at24c); +} diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk new file mode 100644 index 000000000..b3fceceb5 --- /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 000000000..a2a0c109c --- /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 458bb2259..7d9484d3e 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 65f291f40..580fdc62f 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 e4eb1d143..000000000 --- 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 03c713987..000000000 --- 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 68a080c77..acf28c35b 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/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp new file mode 100644 index 000000000..50dece816 --- /dev/null +++ b/src/systemcmds/tests/test_conv.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file test_conv.cpp + * Tests conversions used across the system. + * + */ + +#include + +#include + +#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"); + + 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; + } + } + + warnx("All conversions clean"); + + return 0; +} diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1..96be1e8df 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 000000000..693a208ba --- /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 4da86042d..2a47551ee 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 000000000..44e34d9ef --- /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 000000000..bac9efbdb --- /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 72619fc8b..6a602ecfc 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 a57d04be3..ac64ad33d 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 1088a4407..73827b7cf 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} }; -- cgit v1.2.3 From 1e3d2acbf66b1101a9b17f97d2b786ffaa0e423a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 19:30:23 +0100 Subject: 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 | 557 ++++++++++++++------------- src/modules/mavlink/mavlink_orb_listener.h | 94 +++-- src/modules/mavlink/mavlink_receiver.cpp | 24 +- src/modules/mavlink/mavlink_receiver.h | 4 +- 6 files changed, 426 insertions(+), 321 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1c7986cbb..cd37c5437 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 244af04a6..3b6714559 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 44bf77bb0..fa1a6887e 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,86 +82,97 @@ 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}, -}; - - _n_listeners = sizeof(listeners) / sizeof(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); } +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 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); - - last_sensor_timestamp = raw.timestamp; + orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - /* 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 (gyro_counter != raw.gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; - } - - if (mag_counter != raw.magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; - } - - if (baro_counter != raw.baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; - } - if (gcs_link) - mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp, + // 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], @@ -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); - - 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); + orb_copy(ORB_ID(vehicle_global_position), _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), mavlink_subs.local_pos_sub, &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); + orb_copy(ORB_ID(vehicle_local_position), _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), 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]; + + struct listener* next = _listeners; + unsigned i = 0; - for (unsigned i = 0; i < _n_listeners; i++) { - fds[i].fd = *listeners[i].subp; + 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; } +void * MavlinkOrbListener::uorb_start_helper(void *context) +{ + return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL); +} + pthread_t -MavlinkOrbListener::uorb_receive_start(void) +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 29e081b36..3988103bc 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 4f763c3c6..982d6c1d8 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() } +void * MavlinkReceiver::start_helper(void *context) +{ + return ((MavlinkReceiver *)context)->receive_thread(NULL); +} + pthread_t -MavlinkReceiver::receive_start(int uart) +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 ea57714d2..483d91e72 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: -- cgit v1.2.3 From 63b18399c26acb1e3cf771376c3376b4d00a407a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 21:05:00 +0100 Subject: 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 | 122 ++++++++++++++------------- 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, 105 insertions(+), 100 deletions(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 1fae3ee9d..374d1511c 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 cd37c5437..6c04d2aba 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_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::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 3b6714559..e50b0f0c0 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 fa1a6887e..a6bcd4c0a 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 3988103bc..c9f35a1fb 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 982d6c1d8..0f1d5293e 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 483d91e72..be32ce0f7 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 2a005565e..76798eb12 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 -- cgit v1.2.3 From c73fbca0ebef6a0333b364bfe90891b580eccace Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:13 +0100 Subject: 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 6c04d2aba..f70c3ce75 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 e50b0f0c0..9e2475e2b 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); -- cgit v1.2.3 From a5e02db6d55f246ae71bd5de356dfe17b89bf26d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:36 +0100 Subject: 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 a6bcd4c0a..ffff1838c 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; -- cgit v1.2.3 From b1e5304a3f50975a1d282bf5b7418ff276a26c45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:53 +0100 Subject: 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 0f1d5293e..3752bde10 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; -- cgit v1.2.3 From 62d3369dc94458ed30b9326814296cd9a33a3cc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 11:06:53 +0100 Subject: 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 6c04d2aba..c29d9957d 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 e50b0f0c0..34e2b59ee 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 -- cgit v1.2.3 From 1e6011cc87042b40cf02705deb33f29f3afdb5a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 16:59:39 +0100 Subject: 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 0cd8a0e04..3d8be089e 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 -- cgit v1.2.3 From 8d2d171bb83c4f24f709c20ce8c89b6d1f529616 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 17:00:16 +0100 Subject: 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 fa0575866..b5bf9ece0 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"); -- cgit v1.2.3 From 29b0678d841666000fd67cc955dba5cf3cc980c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 11:56:01 +0100 Subject: navigator: forbid READY - > RTL transition --- src/modules/navigator/navigator_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd..abd7119f7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -689,7 +689,7 @@ Navigator::task_main() if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -748,7 +748,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -756,9 +756,7 @@ Navigator::task_main() break; case NAV_STATE_LAND: - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); - } + dispatch(EVENT_LAND_REQUESTED); break; -- cgit v1.2.3 From 1d40582cc0301f0af330fe69ee63aa8e3d301214 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 12:42:20 +0100 Subject: navigator: forbid READY - > RTL transition fix --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index abd7119f7..6f0fec917 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -965,7 +965,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* 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_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* 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}, -- cgit v1.2.3 From a5045ccee663c500011b8a5a94554f4cbb263352 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 14:38:18 +0100 Subject: 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 b5bf9ece0..21ba51b21 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 bf8c63d38..8c9430829 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 ffff1838c..6d64569de 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 3752bde10..94d97fc12 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(); -- cgit v1.2.3 From 19105bebc58a045446746813a50bf74faaa3ad39 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 16:16:57 +0100 Subject: 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 21ba51b21..cb620349d 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 8c9430829..fb7112891 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. -- cgit v1.2.3 From 6631ecf04a0592c816dfb832fa928a95877184d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 11:45:35 +0100 Subject: commander: reset blink_msg_end when blink message completed to set normal LED status immediately --- src/modules/commander/commander_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88..04624275f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -161,6 +161,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { -- cgit v1.2.3 From 855944fb2eab33074537b5ccfc82e2462b662b5b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 00:24:40 +0100 Subject: commander: beeps and blinks cleanup --- .../commander/accelerometer_calibration.cpp | 2 +- src/modules/commander/airspeed_calibration.cpp | 2 +- src/modules/commander/commander.cpp | 46 +++++------- src/modules/commander/commander_helper.cpp | 86 +++++++++++++--------- src/modules/commander/commander_helper.h | 14 +--- 5 files changed, 76 insertions(+), 74 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 36b75dd58..1cbdf9bf8 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (double)accel_ref[orient][2]); data_collected[orient] = true; - tune_neutral(); + tune_neutral(true); } close(sensor_combined_sub); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 1809f9688..6039d92a7 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd) } mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(); + tune_neutral(true); close(diff_pres_sub); return OK; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b8573..193a7473e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1101,7 +1101,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; - tune_positive(); + tune_positive(true); } } @@ -1196,8 +1196,9 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine according to mode switches */ res = set_main_state_rc(&status); + /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { - tune_positive(); + tune_positive(armed.armed); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1253,7 +1254,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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(); + tune_positive(armed.armed); } } @@ -1308,21 +1309,18 @@ int commander_thread_main(int argc, char *argv[]) /* play arming and battery warning tunes */ if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ - if (tune_arm() == OK) - arm_tune_played = true; + set_tune(TONE_ARMING_WARNING_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ - if (tune_low_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ - if (tune_critical_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_FAST_TUNE); } else if (battery_tune_played) { - tune_stop(); + set_tune(TONE_STOP_TUNE); battery_tune_played = false; } @@ -1693,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg) sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - // only buzz if armed, because else we're driving people nuts indoors - // they really need to look at the leds as well. - if (status->arming_state == ARMING_STATE_ARMED) { - tune_negative(); - } else { - - // Always show the led indication - led_negative(); - } + /* only buzz if armed, because else we're driving people nuts indoors + they really need to look at the leds as well. */ + tune_negative(armed.armed); } } @@ -1715,7 +1707,7 @@ print_reject_arm(const char *msg) char s[80]; sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + tune_negative(true); } } @@ -1723,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); - tune_negative(); + tune_negative(true); break; default: @@ -1883,9 +1875,9 @@ void *commander_low_prio_loop(void *arg) } if (calib_ret == OK) - tune_positive(); + tune_positive(true); else - tune_negative(); + tune_negative(true); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 04624275f..265c0134c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -82,10 +83,21 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) } static int buzzer; -static hrt_abstime blink_msg_end; +static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message +static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence +static int tune_current = 0; // currently playing tune, can be interrupted after tune_end +static int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { + tune_end = 0; + tune_current = 0; + memset(tune_durations, 0, sizeof(tune_durations)); + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { @@ -93,6 +105,8 @@ int buzzer_init() return ERROR; } + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + return OK; } @@ -101,58 +115,60 @@ void buzzer_deinit() close(buzzer); } -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +void set_tune(int tune) { + int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ + if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + /* allow interrupting current non-repeating tune by the same tune */ + if (tune != tune_current || new_tune_duration > 0) { + ioctl(buzzer, TONE_SET_ALARM, tune); + } + tune_current = tune; + if (new_tune_duration > 0) { + tune_end = hrt_absolute_time() + new_tune_duration; + } else { + tune_end = 0; + } + } } -void tune_positive() +/** + * Blink green LED and play positive tune (if use_busser == true). + */ +void tune_positive(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } } -void tune_neutral() +/** + * Blink white LED and play neutral tune (if use_busser == true). + */ +void tune_neutral(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); -} - -void tune_negative() -{ - led_negative(); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } } -void led_negative() +/** + * Blink red LED and play negative tune (if use_busser == true). + */ +void tune_negative(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); -} - -int tune_critical_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); -} - -void tune_stop() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + } } int blink_msg_state() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index af25a5e97..e75f2592f 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); -int tune_arm(void); -int tune_low_bat(void); -int tune_critical_bat(void); -void tune_stop(void); - -void led_negative(); +void set_tune(int tune); +void tune_positive(bool use_buzzer); +void tune_neutral(bool use_buzzer); +void tune_negative(bool use_buzzer); int blink_msg_state(); -- cgit v1.2.3 From 0613b299c033c21e1ddcbd8faeb9d3430d72386c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 09:13:51 +0100 Subject: commander: play warning tune (as for low battery) when in failsafe state --- src/modules/commander/commander.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 193a7473e..20bfca477 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,14 +1311,14 @@ int commander_thread_main(int argc, char *argv[]) /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* play tune on battery warning */ - set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* play tune on battery warning or failsafe */ + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); + } else if (battery_tune_played) { set_tune(TONE_STOP_TUNE); battery_tune_played = false; @@ -1420,11 +1420,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - rgbled_set_color(RGBLED_COLOR_AMBER); - } - + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { -- cgit v1.2.3 From 0ead560059fff0a31183f40a6b848e82aa2dae80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 18:24:20 +0100 Subject: commander: tunes cleanup and fixes --- src/modules/commander/commander.cpp | 9 +++------ src/modules/commander/commander_helper.cpp | 32 ++++++++++++++---------------- 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20bfca477..7284b38c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -607,7 +607,6 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; - bool battery_tune_played = false; bool arm_tune_played = false; /* set parameters */ @@ -1020,14 +1019,12 @@ int commander_thread_main(int argc, char *argv[]) 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 */ 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); @@ -1310,6 +1307,7 @@ int commander_thread_main(int argc, char *argv[]) if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); + arm_tune_played = true; } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ @@ -1319,13 +1317,12 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (battery_tune_played) { + } else { set_tune(TONE_STOP_TUNE); - battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { + if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 265c0134c..fe6c9bfaa 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,20 +82,20 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -static int buzzer; +static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence -static int tune_current = 0; // currently playing tune, can be interrupted after tune_end -static int tune_durations[TONE_NUMBER_OF_TUNES]; +static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end +static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { tune_end = 0; tune_current = 0; memset(tune_durations, 0, sizeof(tune_durations)); - tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); @@ -105,8 +105,6 @@ int buzzer_init() return ERROR; } - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - return OK; } @@ -116,15 +114,15 @@ void buzzer_deinit() } void set_tune(int tune) { - int new_tune_duration = tune_durations[tune]; + unsigned int new_tune_duration = tune_durations[tune]; /* don't interrupt currently playing non-repeating tune by repeating */ - if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ - if (tune != tune_current || new_tune_duration > 0) { + if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; - if (new_tune_duration > 0) { + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; } else { tune_end = 0; @@ -133,7 +131,7 @@ void set_tune(int tune) { } /** - * Blink green LED and play positive tune (if use_busser == true). + * Blink green LED and play positive tune (if use_buzzer == true). */ void tune_positive(bool use_buzzer) { @@ -146,7 +144,7 @@ void tune_positive(bool use_buzzer) } /** - * Blink white LED and play neutral tune (if use_busser == true). + * Blink white LED and play neutral tune (if use_buzzer == true). */ void tune_neutral(bool use_buzzer) { @@ -159,7 +157,7 @@ void tune_neutral(bool use_buzzer) } /** - * Blink red LED and play negative tune (if use_busser == true). + * Blink red LED and play negative tune (if use_buzzer == true). */ void tune_negative(bool use_buzzer) { @@ -185,8 +183,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() { -- cgit v1.2.3 From ea2a69d8bf15cdb0c4a3234ed2851f9380d5adfc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 22:36:36 +0100 Subject: Mavlink: get orb_listener to work --- src/modules/mavlink/mavlink_main.cpp | 29 +++++++++-- src/modules/mavlink/mavlink_orb_listener.cpp | 74 +++++++++++++++------------- src/modules/mavlink/mavlink_orb_listener.h | 6 +-- src/modules/uORB/topics/vehicle_attitude.h | 2 +- 4 files changed, 66 insertions(+), 45 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb620349d..f1ec6e8dc 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 6d64569de..418e1c121 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; - - // Register it - struct listener *next = _listeners; - while (next->next != nullptr) { - next = next->next; + 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; } - - // 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 c9f35a1fb..349a11f2e 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 e5a35ff9b..40328af14 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,7 +76,7 @@ struct vehicle_attitude_s { float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ - float g_comp[3]; /**< Compensated gravity vector */ + float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ -- cgit v1.2.3 From 4b567ef6310c07d46307437ac9ba76c76a5b2c69 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 23:59:02 +0100 Subject: gpio_led: bugs fixed, PX4FMUv2 support added --- makefiles/config_px4fmu-v2_default.mk | 1 + src/modules/gpio_led/gpio_led.c | 80 +++++++++++++++++++++++------------ 2 files changed, 55 insertions(+), 26 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..3a3743893 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,6 +71,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard +MODULES += modules/gpio_led # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index d383146f9..f5f3dea76 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -63,8 +62,6 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; - struct actuator_armed_s armed; - int actuator_armed_sub; bool led_state; int counter; }; @@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { if (argc < 2) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n" "\t-p\tUse pin:\n" "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" @@ -88,7 +86,15 @@ int gpio_led_main(int argc, char *argv[]) "\t\ta1\tPX4IO ACC1\n" "\t\ta2\tPX4IO ACC2\n" "\t\tr1\tPX4IO RELAY1\n" - "\t\tr2\tPX4IO RELAY2"); + "\t\tr2\tPX4IO RELAY2" + ); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "usage: gpio_led {start|stop} [-p ]\n" + "\t-p\tUse pin:\n" + "\t\tn\tAUX OUT pin number (default: 1)\n" + ); +#endif } else { @@ -98,10 +104,14 @@ int gpio_led_main(int argc, char *argv[]) } bool use_io = false; - int pin = GPIO_EXT_1; + + /* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */ + int pin = 1; if (argc > 2) { if (!strcmp(argv[2], "-p")) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (!strcmp(argv[3], "1")) { use_io = false; pin = GPIO_EXT_1; @@ -129,6 +139,20 @@ int gpio_led_main(int argc, char *argv[]) } else { errx(1, "unsupported pin: %s", argv[3]); } + +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + unsigned int n = strtoul(argv[3], NULL, 10); + + if (n >= 1 && n <= 6) { + use_io = false; + pin = n; + + } else { + errx(1, "unsupported pin: %s", argv[3]); + } + +#endif } } @@ -144,6 +168,8 @@ int gpio_led_main(int argc, char *argv[]) gpio_led_started = true; char pin_name[24]; +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (use_io) { if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); @@ -154,9 +180,13 @@ int gpio_led_main(int argc, char *argv[]) } else { sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); - } +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + sprintf(pin_name, "AUX OUT %i", pin); +#endif + warnx("start, using pin: %s", pin_name); } @@ -186,6 +216,7 @@ void gpio_led_start(FAR void *arg) if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; + } else { gpio_dev = PX4FMU_DEVICE_PATH; } @@ -204,8 +235,10 @@ void gpio_led_start(FAR void *arg) /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); - /* subscribe to vehicle status topic */ + /* initialize vehicle status structure */ memset(&priv->status, 0, sizeof(priv->status)); + + /* subscribe to vehicle status topic */ priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* add worker to queue */ @@ -224,38 +257,32 @@ void gpio_led_cycle(FAR void *arg) FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; /* check for status updates*/ - bool status_updated; - orb_check(priv->vehicle_status_sub, &status_updated); + bool updated; + orb_check(priv->vehicle_status_sub, &updated); - if (status_updated) + if (updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); - orb_check(priv->vehicle_status_sub, &status_updated); - - if (status_updated) - orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); - /* select pattern for current status */ int pattern = 0; - if (priv->armed.armed) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) { + pattern = 0x2A; // *_*_*_ fast blink (armed, error) + + } else if (priv->status.arming_state == ARMING_STATE_ARMED) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { pattern = 0x3f; // ****** solid (armed) } else { - pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe) } - } else { - if (priv->armed.ready_to_arm) { - pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (priv->status.arming_state == ARMING_STATE_STANDBY) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) - } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { - pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) { + pattern = 0x28; // *_*___ slow double blink (disarmed, error) - } else { - pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) - } } /* blink pattern */ @@ -266,6 +293,7 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); + } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } -- cgit v1.2.3 From 36bd7a797b6116b2f4b1c3fe358ee56eb7982b1d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 11:46:26 +0100 Subject: navigator: use bearing to home for RTL --- src/modules/navigator/navigator_main.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd..d7f6fd16a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1343,7 +1343,14 @@ Navigator::set_rtl_item() _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 + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); + } _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; @@ -1409,17 +1416,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + if (_pos_sp_triplet.previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); + + } else { + /* else use current position */ + sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); + } + sp->loiter_radius = _parameters.loiter_radius; + sp->loiter_direction = 1; + sp->pitch_min = 0.0f; + } 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; } - 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; -- cgit v1.2.3 From 99b426c27c004b6430c4058151b0dd3854989414 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 12:38:35 +0100 Subject: 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 5054937e0..0ea655cac 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); */ #define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); + struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; unsigned char severity; @@ -112,6 +113,7 @@ struct mavlink_logbuffer { struct mavlink_logmessage *elems; }; +__BEGIN_DECLS void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); @@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...); +__END_DECLS #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f1ec6e8dc..4f4d94947 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 fb7112891..9b289c965 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; /** -- cgit v1.2.3 From 76ae004e5c6c1dcc05f1eb784f6dc14cff2a3671 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 16:21:50 +0100 Subject: 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 1dc96b3c3..2e82b8a2d 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 2a734c27e..050872677 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 -- cgit v1.2.3 From cc5756f61ffa9b84abcbebfcf89d213d99d100d9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 18:35:45 +0100 Subject: 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 2585593ea..7e146604b 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 ce2d2b34a..b812af95a 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; -- cgit v1.2.3 From 61a849bf6ba11b98b7332f89d0b32226601f4d63 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 19:13:57 +0100 Subject: 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 7e146604b..cbbe3c31f 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 b812af95a..bf7675267 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, -- cgit v1.2.3 From 4a66e285adcd88cf42c3907753abd9b433815e45 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 00:05:27 +0100 Subject: navigator mavlink log info messages: add #audio tag --- src/modules/navigator/navigator_main.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5139ae6cd..2711be24f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,7 +865,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] nav state: %s", nav_states_str[myState]); prevState = myState; /* reset time counter on state changes */ @@ -1073,11 +1073,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < 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)); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter at current altitude"); } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; @@ -1175,14 +1175,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [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); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to offboard WP %d", index); } } @@ -1331,7 +1331,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1357,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1384,12 +1384,12 @@ Navigator::set_rtl_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); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); break; } @@ -1516,7 +1516,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } @@ -1541,7 +1541,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff completed"); } else { /* advance by one mission item */ -- cgit v1.2.3 From 346ae5b9f4fc2da13e6d890521f48768b6b6e8c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 19:13:10 +0100 Subject: 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 cbbe3c31f..0d9d06190 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 bf7675267..d5bbb746b 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 cd9408f2f..a2b71c931 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 560b47423..317a489d4 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 89dcb1d7b..16a1b9aff 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 d8d3b5452..6614e13f4 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; -- cgit v1.2.3 From ef46cd5e909115c8c208c409fcded0dd02037d5e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 20:54:10 +0100 Subject: 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 16a1b9aff..44fff2416 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 { -- cgit v1.2.3 From 523637e0f1fb0247111818d0a88ce8c4574728ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 13:36:59 +0100 Subject: 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 0d9d06190..79aa6135b 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 d5bbb746b..c667a41da 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 a2b71c931..fdc196371 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 317a489d4..26a2e832f 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 44fff2416..371f945c4 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 6614e13f4..8e139e5e4 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); -- cgit v1.2.3 From 71cd1326635bead6c788b8967c6d5bcfd920b49c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 14:24:26 +0100 Subject: 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 79aa6135b..d2c94ca92 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")) { -- cgit v1.2.3 From 30dacbd1541073f9e35b8de0d1410e23f3aaccda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Feb 2014 11:29:36 +0100 Subject: 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 d2c94ca92..38f753c39 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 c667a41da..9461a51f8 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 260356eca..e1dde35bf 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 e72caf98e..fdc4ffff6 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 15d4e86bf..845c16583 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 c99706b97..7c3921198 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -54,7 +54,8 @@ struct mission_result_s { bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index; /**< index of the mission which has been reached */ + unsigned mission_index_reached; /**< index of the mission which has been reached */ + unsigned index_current_mission; /**< index of the current mission */ }; /** -- cgit v1.2.3 From b596bf6aa56b2039b51a03ab11acf2d7d719195d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 13:24:02 +0100 Subject: 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 38f753c39..42504dea9 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 371f945c4..d07de0f22 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 8e139e5e4..fca5de917 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; }; -- cgit v1.2.3 From 366f19c2c07f249a6ac51f16f08b86454e2e2da4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 13:24:56 +0100 Subject: 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 2e82b8a2d..7aa71f182 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 050872677..d42b23ab0 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 -- cgit v1.2.3 From a61a89f339394dd87de5c48951fb12d118b14e7c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Feb 2014 00:25:30 +0100 Subject: ROMFS: ignore comments and newlines in startup files, text in mixer files --- Tools/px_romfs_pruner.py | 83 ++++++++++++++++++++++++++++++++++++++++++++++++ makefiles/firmware.mk | 4 +++ 2 files changed, 87 insertions(+) create mode 100644 Tools/px_romfs_pruner.py diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py new file mode 100644 index 000000000..f6ed0a8e4 --- /dev/null +++ b/Tools/px_romfs_pruner.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2014 PX4 Development Team. All rights reserved. +# 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. +# +############################################################################ + + +""" +px_romfs_pruner.py: +Delete all comments and newlines before ROMFS is converted to an image +""" + +from __future__ import print_function +import argparse +import os + +def main(): + + # Parse commandline arguments + parser = argparse.ArgumentParser(description="ROMFS pruner.") + parser.add_argument('--folder', action="store", help="ROMFS scratch folder.") + args = parser.parse_args() + + print("Pruning ROMFS files.") + + # go through + for (root, dirs, files) in os.walk(args.folder): + for file in files: + # only prune text files + if ".zip" in file or ".bin" in file: + continue + + file_path = os.path.join(root, file) + + # read file line by line + pruned_content = "" + with open(file_path, "r") as f: + for line in f: + + # handle mixer files differently than startup files + if ".mix" in file_path: + if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): + pruned_content += line + else: + if not line.isspace() and not line.strip().startswith("#"): + pruned_content += line + + # overwrite old scratch file + with open(file_path, "w") as f: + f.write(pruned_content) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index cb20d9cd1..1b646d9e0 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -355,6 +355,9 @@ ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) LINK_DEPS += $(ROMFS_OBJ) +# Remove all comments from startup and mixer files +ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py + # Turn the ROMFS image into an object file $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) $(call BIN_TO_OBJ,$<,$@,romfs_img) @@ -372,6 +375,7 @@ ifneq ($(ROMFS_EXTRA_FILES),) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras endif + $(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH) EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) -- cgit v1.2.3 From 94c40b6fa44df7d9756a4f318f6bce1de9bedd89 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Feb 2014 09:06:35 +0100 Subject: ROMFS: only identify *.mix as mixer files --- Tools/px_romfs_pruner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index f6ed0a8e4..ceef9f9be 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -67,7 +67,7 @@ def main(): for line in f: # handle mixer files differently than startup files - if ".mix" in file_path: + if file_path.endswith(".mix"): if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): pruned_content += line else: -- cgit v1.2.3 From 8bc0287eccc7871db17c199a3b330e74c92c068a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 22:45:54 +0400 Subject: commander: disarm system when safety enabled --- src/modules/commander/commander.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6d14472f3..29a930ebb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,11 +902,13 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); - // XXX this would be the right approach to do it, but do we *WANT* this? - // /* disarm if safety is now on and still armed */ - // if (safety.safety_switch_available && !safety.safety_off) { - // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - // } + /* disarm if safety is now on and still armed */ + if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + } + } } /* update global position estimate */ -- cgit v1.2.3 From acf680389e95c82d2e0df1253f16842d320f3504 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Feb 2014 19:48:45 +0100 Subject: launchdetector: add reset, #663 --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 8 +++++++- src/lib/launchdetection/CatapultLaunchMethod.h | 3 +-- src/lib/launchdetection/LaunchDetector.cpp | 6 ++++++ src/lib/launchdetection/LaunchDetector.h | 1 + src/lib/launchdetection/LaunchMethod.h | 1 + src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 6 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index d5c759b17..1039b4a09 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -42,7 +42,7 @@ #include CatapultLaunchMethod::CatapultLaunchMethod() : - last_timestamp(0), + last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), threshold_accel(NULL, "LAUN_CAT_A", false), @@ -88,3 +88,9 @@ void CatapultLaunchMethod::updateParams() threshold_accel.update(); threshold_time.update(); } + +void CatapultLaunchMethod::reset() +{ + integrator = 0.0f; + launchDetected = false; +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index e943f11e9..b8476b4c8 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -55,11 +55,10 @@ public: void update(float accel_x); bool getLaunchDetected(); void updateParams(); + void reset(); private: hrt_abstime last_timestamp; -// float threshold_accel_raw; -// float threshold_time; float integrator; bool launchDetected; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index df9f2fe95..4109a90ba 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -59,6 +59,12 @@ LaunchDetector::~LaunchDetector() } +void LaunchDetector::reset() +{ + /* Reset all detectors */ + launchMethods[0]->reset(); +} + void LaunchDetector::update(float accel_x) { if (launchdetection_on.get() == 1) { diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 7c2ff826c..05708c526 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -53,6 +53,7 @@ class __EXPORT LaunchDetector public: LaunchDetector(); ~LaunchDetector(); + void reset(); void update(float accel_x); bool getLaunchDetected(); diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index bfb5f8cb4..0cfbab3e0 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -47,6 +47,7 @@ public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; virtual void updateParams() = 0; + virtual void reset() = 0; protected: private: }; 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 45fdaa355..a4142266b 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 @@ -1055,6 +1055,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; + launchDetector.reset(); } if (was_circle_mode && !_l1_control.circle_mode()) { -- cgit v1.2.3 From 4b519e4d5daee4da8e619414d4b1cd697198b6a3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:04:11 +0100 Subject: Navigator: Get rid of warnings first --- src/modules/navigator/navigator_main.cpp | 46 ++++++++++++++++---------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 577c767a8..f59663ca0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -154,17 +154,16 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ - orb_advert_t _pos_sp_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 */ struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + 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 position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + 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 */ + struct mission_item_s _mission_item; /**< current mission item */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -178,21 +177,22 @@ private: class Mission _mission; - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ + bool _mission_item_valid; /**< current mission item valid */ + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ bool _pos_sp_triplet_updated; - char *nav_states_str[NAV_STATE_MAX]; + const char *nav_states_str[NAV_STATE_MAX]; struct { float min_altitude; @@ -382,11 +382,11 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _control_mode_sub(-1), /* publications */ _pos_sp_triplet_pub(-1), @@ -395,22 +395,22 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), -/* states */ - _rtl_state(RTL_STATE_NONE), + _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _mission_item_valid(false), _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0), - _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _set_nav_state_timestamp(0), _pos_sp_triplet_updated(false), - _geofence_violation_warning_sent(false) +/* states */ + _rtl_state(RTL_STATE_NONE) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); @@ -1162,7 +1162,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); } else { if (onboard) { @@ -1318,7 +1318,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } @@ -1344,7 +1344,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1362,7 +1362,7 @@ Navigator::set_rtl_item() _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.time_inside = _parameters.rtl_land_delay < 0.0f ? 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; @@ -1371,7 +1371,7 @@ Navigator::set_rtl_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); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1525,7 +1525,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } } -- cgit v1.2.3 From 3eaa892ee42f7de84698d46ef8cf3b29b96a2b34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:35:41 +0100 Subject: fw_pos_control_l1: indentation only --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 32 +++++++++++----------- 1 file changed, 16 insertions(+), 16 deletions(-) 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 45fdaa355..10aa89088 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 @@ -130,23 +130,23 @@ private: int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _sensor_combined_sub; /**< for body frame accelerations */ + int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - 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 position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ - struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + 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 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 */ @@ -192,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::Matrix<3, 3> _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -406,8 +406,8 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - position_setpoint_triplet_s _pos_sp_triplet = {0}; - sensor_combined_s _sensor_combined = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -587,8 +587,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7f; - _launch_lon = _global_pos.lon / 1e7f; + _launch_lat = _global_pos.lat / 1e7d; + _launch_lon = _global_pos.lon / 1e7d; _launch_alt = _global_pos.alt; _launch_valid = true; } -- cgit v1.2.3 From 83f66e45999c7db8734534ba1cfb9f9e023f47a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Feb 2014 14:40:11 +0100 Subject: fw_pos_control_l1: use double everywhere for lat and lon --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 10aa89088..c1aa8d39c 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 @@ -154,13 +154,13 @@ private: /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; + double _loiter_hold_lat; + double _loiter_hold_lon; float _loiter_hold_alt; bool _loiter_hold; - float _launch_lat; - float _launch_lon; + double _launch_lat; + double _launch_lon; float _launch_alt; bool _launch_valid; @@ -587,8 +587,8 @@ FixedwingPositionControl::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); if (!was_armed && _control_mode.flag_armed) { - _launch_lat = _global_pos.lat / 1e7d; - _launch_lon = _global_pos.lon / 1e7d; + _launch_lat = _global_pos.lat; + _launch_lon = _global_pos.lon; _launch_alt = _global_pos.alt; _launch_valid = true; } -- cgit v1.2.3 From a43c7c488eaba5022a18f05e590059cad6f580da Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 18:54:58 +0400 Subject: navigator: "reached" flags reset fixed --- src/modules/navigator/navigator_main.cpp | 39 +++++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ed12d106..af1b9aac0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -321,6 +321,11 @@ private: */ bool onboard_mission_available(unsigned relative_index); + /** + * Reset all "reached" flags. + */ + void reset_reached(); + /** * Check if current mission item has been reached. */ @@ -855,9 +860,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - - /* reset time counter on state changes */ - _time_first_inside_orbit = 0; } perf_end(_loop_perf); @@ -1009,6 +1011,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { void Navigator::start_none() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -1024,6 +1028,8 @@ Navigator::start_none() void Navigator::start_ready() { + reset_reached(); + _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; @@ -1046,6 +1052,8 @@ Navigator::start_ready() void Navigator::start_loiter() { + reset_reached(); + _do_takeoff = false; /* set loiter position if needed */ @@ -1091,6 +1099,8 @@ Navigator::start_mission() void Navigator::set_mission_item() { + reset_reached(); + /* copy current mission to previous item */ memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); @@ -1104,9 +1114,6 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - /* reset time counter for new item */ - _time_first_inside_orbit = 0; - _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1224,6 +1231,8 @@ Navigator::start_rtl() void Navigator::start_land() { + reset_reached(); + /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ _do_takeoff = false; @@ -1255,6 +1264,8 @@ Navigator::start_land() void Navigator::start_land_home() { + reset_reached(); + /* land to home position, should be called when hovering above home, from RTL state */ _do_takeoff = false; _reset_loiter_pos = true; @@ -1285,8 +1296,7 @@ Navigator::start_land_home() void Navigator::set_rtl_item() { - /*reset time counter for new RTL item */ - _time_first_inside_orbit = 0; + reset_reached(); switch (_rtl_state) { case RTL_STATE_CLIMB: { @@ -1550,9 +1560,7 @@ Navigator::check_mission_item_reached() /* check if the MAV was long enough inside the waypoint orbit */ 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; + reset_reached(); return true; } } @@ -1561,6 +1569,15 @@ Navigator::check_mission_item_reached() } +void +Navigator::reset_reached() +{ + _time_first_inside_orbit = 0; + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + +} + void Navigator::on_mission_item_reached() { -- cgit v1.2.3 From 7d8f08ad9a733111a9d8b1512a85592a2fb9b045 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 19:05:32 +0400 Subject: navigator: check if yaw reached only when position reached --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index af1b9aac0..4cd60d8d8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1533,7 +1533,7 @@ Navigator::check_mission_item_reached() } } - if (!_waypoint_yaw_reached) { + if (_waypoint_position_reached && !_waypoint_yaw_reached) { 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.yaw - _global_pos.yaw); -- cgit v1.2.3 From 79d2e5de6cf00186359e5b436c602055e4110249 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:48:31 +0100 Subject: TECS: use constant from geo --- src/lib/ecl/ecl.h | 3 +-- src/lib/external_lgpl/tecs/tecs.cpp | 5 +---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index e0f207696..aa3c5000a 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,7 +38,6 @@ */ #include -#include #define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file +#define ecl_elapsed_time hrt_elapsed_time diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0f28bccad..3730b1920 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -3,13 +3,10 @@ #include "tecs.h" #include #include +#include using namespace math; -#ifndef CONSTANTS_ONE_G -#define CONSTANTS_ONE_G GRAVITY -#endif - /** * @file tecs.cpp * -- cgit v1.2.3 From 84df8ee78d28c51fc0f6272771d3b7c12b281cfe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:48:46 +0100 Subject: TECS: warning fixes --- src/lib/external_lgpl/tecs/tecs.h | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index d1ebacda1..5cafb1c79 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,16 +28,7 @@ class __EXPORT TECS { public: TECS() : - - _airspeed_enabled(false), - _throttle_slewrate(0.0f), - _climbOutDem(false), - _hgt_dem_prev(0.0f), - _hgt_dem_adj_last(0.0f), - _hgt_dem_in_old(0.0f), - _TAS_dem_last(0.0f), - _TAS_dem_adj(0.0f), - _TAS_dem(0.0f), + _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), _integ3_state(0.0f), @@ -45,8 +36,16 @@ public: _integ5_state(0.0f), _integ6_state(0.0f), _integ7_state(0.0f), - _pitch_dem(0.0f), _last_pitch_dem(0.0f), + _vel_dot(0.0f), + _TAS_dem(0.0f), + _TAS_dem_last(0.0f), + _hgt_dem_in_old(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_prev(0.0f), + _TAS_dem_adj(0.0f), + _STEdotErrLast(0.0f), + _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), _SPEdot_dem(0.0f), @@ -55,9 +54,9 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), - _vel_dot(0.0f), - _STEdotErrLast(0.0f) { - + _airspeed_enabled(false), + _throttle_slewrate(0.0f) + { } bool airspeed_sensor_enabled() { -- cgit v1.2.3 From 18f71e6bc41e9823f8c82c49c03ee8c7261b8053 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Feb 2014 10:49:03 +0100 Subject: fw_pos_control_l1: warning fixes --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 53 +++++++++++----------- 1 file changed, 27 insertions(+), 26 deletions(-) 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 c1aa8d39c..62c69d3eb 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 @@ -362,6 +362,7 @@ FixedwingPositionControl *g_control; FixedwingPositionControl::FixedwingPositionControl() : + _mavlink_fd(-1), _task_should_exit(false), _control_task(-1), @@ -380,36 +381,34 @@ FixedwingPositionControl::FixedwingPositionControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), + /* states */ _setpoint_valid(false), _loiter_hold(false), - _airspeed_error(0.0f), - _airspeed_valid(false), - _groundspeed_undershoot(0.0f), - _global_pos_valid(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), land_onslope(false), + launch_detected(false), + usePreTakeoffThrust(false), flare_curve_alt_last(0.0f), - _mavlink_fd(-1), launchDetector(), - launch_detected(false), - usePreTakeoffThrust(false) + _airspeed_error(0.0f), + _airspeed_valid(false), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false) { /* safely initialize structs */ - vehicle_attitude_s _att = {0}; - vehicle_attitude_setpoint_s _att_sp = {0}; - navigation_capabilities_s _nav_capabilities = {0}; - manual_control_setpoint_s _manual = {0}; - airspeed_s _airspeed = {0}; - vehicle_control_mode_s _control_mode = {0}; - vehicle_global_position_s _global_pos = {0}; - position_setpoint_triplet_s _pos_sp_triplet = {0}; - sensor_combined_s _sensor_combined = {0}; - - + memset(&_att, 0, sizeof(vehicle_attitude_s)); + memset(&_att_sp, 0, sizeof(vehicle_attitude_setpoint_s)); + memset(&_nav_capabilities, 0, sizeof(navigation_capabilities_s)); + memset(&_manual, 0, sizeof(manual_control_setpoint_s)); + memset(&_airspeed, 0, sizeof(airspeed_s)); + memset(&_control_mode, 0, sizeof(vehicle_control_mode_s)); + memset(&_global_pos, 0, sizeof(vehicle_global_position_s)); + memset(&_pos_sp_triplet, 0, sizeof(position_setpoint_triplet_s)); + memset(&_sensor_combined, 0, sizeof(sensor_combined_s)); _nav_capabilities.turn_distance = 0.0f; @@ -916,7 +915,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) { flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; @@ -1074,13 +1073,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _seatbelt_hold_heading = _att.yaw + _manual.yaw; } - /* climb out control */ - bool climb_out = false; + //XXX not used - /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { - climb_out = true; - } + /* climb out control */ +// bool climb_out = false; +// +// /* user wants to climb out */ +// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { +// climb_out = true; +// } /* if in seatbelt mode, set airspeed based on manual control */ @@ -1287,7 +1288,7 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; -- cgit v1.2.3 From 6480c6a2cee7a237f9310f6986567e91c54c1247 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 21:48:54 +0100 Subject: fw autoland: remove confusing slopelength parameter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +----- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ------- 2 files changed, 1 insertion(+), 12 deletions(-) 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 45fdaa355..020d1c119 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 @@ -233,7 +233,6 @@ private: float speedrate_p; float land_slope_angle; - float land_slope_length; float land_H1_virt; float land_flare_alt_relative; float land_thrust_lim_alt_relative; @@ -278,7 +277,6 @@ private: param_t speedrate_p; param_t land_slope_angle; - param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; @@ -431,7 +429,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); @@ -520,7 +517,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); - param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); @@ -889,7 +885,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * 0.9f; float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); 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 62a340e90..0909135e1 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 @@ -348,13 +348,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); -/** - * Landing slope length - * - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); - /** * * -- cgit v1.2.3 From b7488da441cf945ec1e5f0a1a4c4b6295707a8b0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 22:06:05 +0100 Subject: fw autoland: completely remove the virtual wp between the last wp and the landing wp. autoland starts now from the last normal wp. --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 32 ++++++++-------------- 1 file changed, 11 insertions(+), 21 deletions(-) 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 020d1c119..e1869328a 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 @@ -885,7 +885,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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(0), prev_wp(1), curr_wp(0), curr_wp(1)) * 0.9f; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); @@ -931,34 +931,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_last = flare_curve_alt; - - } else if (wp_distance < L_wp_distance) { - - /* minimize speed to approach speed, stay on landing slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); - - if (!land_onslope) { - - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); - land_onslope = true; - } - } else { /* intersect glide slope: - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope - * */ + * minimize speed to approach speed + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + + if (!land_onslope) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + land_onslope = true; + } + } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); -- cgit v1.2.3 From 9830f668c548ed63131722cdbba4753e1c06f647 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 22:55:40 +0100 Subject: fw autoland: fix warning --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e1869328a..b0d2bcbc6 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 @@ -912,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) { flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; -- cgit v1.2.3 From ae3e625ce8cec56527dc6809c0b48081773b47a3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:20:17 +0100 Subject: fw pos ctrl: initialize uorb structs in initialization list --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 ++++++++++------------ 1 file changed, 10 insertions(+), 13 deletions(-) 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 62c69d3eb..1feb539b9 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 @@ -397,20 +397,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined() { - /* safely initialize structs */ - memset(&_att, 0, sizeof(vehicle_attitude_s)); - memset(&_att_sp, 0, sizeof(vehicle_attitude_setpoint_s)); - memset(&_nav_capabilities, 0, sizeof(navigation_capabilities_s)); - memset(&_manual, 0, sizeof(manual_control_setpoint_s)); - memset(&_airspeed, 0, sizeof(airspeed_s)); - memset(&_control_mode, 0, sizeof(vehicle_control_mode_s)); - memset(&_global_pos, 0, sizeof(vehicle_global_position_s)); - memset(&_pos_sp_triplet, 0, sizeof(position_setpoint_triplet_s)); - memset(&_sensor_combined, 0, sizeof(sensor_combined_s)); - - _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); -- cgit v1.2.3 From 60de15ea9d753eef3361778633f7f91fb92aa748 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:26:50 +0100 Subject: fw autoland: remove old comments --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ---- 1 file changed, 4 deletions(-) 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 b0d2bcbc6..3367aee9a 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 @@ -942,17 +942,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } - } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), -- cgit v1.2.3 From b7899056f5454d8508a20dff87661c424ad98340 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Feb 2014 23:32:55 +0100 Subject: fw autoland: add very short comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 3367aee9a..64d2b7019 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 @@ -885,8 +885,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); -- cgit v1.2.3 From 2d97dff35c2976c7ba98e97a66b4a141cfb8960e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 06:52:40 +0100 Subject: Added support for DX10t, which apparently outputs 13 channels --- src/modules/px4iofirmware/dsm.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 60eda2319..70f31240a 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -162,6 +162,7 @@ dsm_guess_format(bool reset) 0xff, /* 8 channels (DX8) */ 0x1ff, /* 9 channels (DX9, etc.) */ 0x3ff, /* 10 channels (DX10) */ + 0x1fff, /* 13 channels (DX10t) */ 0x3fff /* 18 channels (DX10) */ }; unsigned votes10 = 0; -- cgit v1.2.3 From 9828d5ede474723d83b189c394397d78c7050b7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 07:50:36 +0100 Subject: Fix copyright, do not return junk channel #13 that Spektrum gives us. --- src/modules/px4iofirmware/dsm.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 70f31240a..0d3eb2606 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.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 @@ -401,6 +401,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } + /* + * Spektrum likes to send junk in higher channel numbers to fill + * their packets. We don't know about a 13 channel model in their TX + * lines, so if we get a channel count of 13, we'll return 12 (the last + * data index that is stable). + */ + if (*num_values == 13) + *num_values = 12; + if (dsm_channel_shift == 11) { /* Set the 11-bit data indicator */ *num_values |= 0x8000; -- cgit v1.2.3 From b58fa2dffc22b3ba27ac87f1f514c298024cea4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 10:37:54 +0100 Subject: Change bit mask to allow for 10 channels. --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1335f52e1..97d25bbfa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_DSM: - dsm_bind(value & 0x0f, (value >> 4) & 7); + dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; default: -- cgit v1.2.3 From f1f1ecd096f2ad7b791127c9ee943a7dfc0ab3f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 10:36:46 +0100 Subject: Fix mavlink feedback FD handling --- src/drivers/px4io/px4io.cpp | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index efcf4d12b..13326e962 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,8 +244,7 @@ private: volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag - int _mavlink_fd; ///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. - int _thread_mavlink_fd; ///< mavlink file descriptor for thread. + int _mavlink_fd; ///< mavlink file descriptor. perf_counter_t _perf_update; /// Date: Tue, 18 Feb 2014 10:37:11 +0100 Subject: Code style --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index c3eea310f..7a93e513e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) -- cgit v1.2.3 From a91b68d8a52709c5ce3ce465cad3b7e5a5e34f66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 10:37:32 +0100 Subject: Fix DSM pairing error handling. --- src/drivers/px4io/px4io.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 13326e962..7c7b3dcb7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2112,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); - usleep(500000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(72000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - usleep(50000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(72000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + usleep(50000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + + ret = OK; + } else { + ret = -EINVAL; + } break; case DSM_BIND_POWER_UP: @@ -2612,7 +2622,7 @@ bind(int argc, char *argv[]) #endif if (argc < 3) - errx(0, "needs argument, use dsm2 or dsmx"); + errx(0, "needs argument, use dsm2, dsmx or dsmx8"); if (!strcmp(argv[2], "dsm2")) pulses = DSM2_BIND_PULSES; @@ -2621,7 +2631,7 @@ bind(int argc, char *argv[]) else if (!strcmp(argv[2], "dsmx8")) pulses = DSMX8_BIND_PULSES; else - errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); // Test for custom pulse parameter if (argc > 3) pulses = atoi(argv[3]); -- cgit v1.2.3 From 06b69b70a5d73ba32bc08cacedbeb9016a32195b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 12:27:04 +0100 Subject: Scaling Spektrum inputs into normalized value same as the servo outputs do --- src/modules/px4iofirmware/dsm.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 0d3eb2606..aac2087b2 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -369,11 +369,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) if (channel >= *num_values) *num_values = channel + 1; - /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (dsm_channel_shift == 11) - value /= 2; + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ + if (dsm_channel_shift == 10) + value *= 2; - value += 998; + /* + * Spektrum scaling is special. There are these basic considerations + * + * * Midpoint is 1520 us + * * 100% travel channels are +- 400 us + * + * We obey the original Spektrum scaling (so a default setup will scale from + * 1100 - 1900 us), but we do not obey the weird 1520 us center point + * and instead (correctly) center the center around 1500 us. This is in order + * to get something useful without requiring the user to calibrate on a digital + * link for no reason. + */ + + /* scaled integer for decent accuracy while staying efficient */ + value = (((value - 1024) * 1000) / 1700) + 1500; /* * Store the decoded channel into the R/C input buffer, taking into -- cgit v1.2.3 From 983cff56b3854aecf645773a1863ac01903b8c5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 12:50:29 +0100 Subject: Added int cast to handle arithmetic correctly --- src/modules/px4iofirmware/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index aac2087b2..d3f365822 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -387,7 +387,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) */ /* scaled integer for decent accuracy while staying efficient */ - value = (((value - 1024) * 1000) / 1700) + 1500; + value = ((((int)value - 1024) * 1000) / 1700) + 1500; /* * Store the decoded channel into the R/C input buffer, taking into -- cgit v1.2.3 From f4bac7e7e802abf671d0ee87ad84b6f281fd81be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 21 Feb 2014 20:22:05 +0100 Subject: navigator: remove all but one [navigator] tag from mavlink audio messages --- src/modules/navigator/navigator_main.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2711be24f..0268c47e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,7 +865,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] nav state: %s", nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); prevState = myState; /* reset time counter on state changes */ @@ -1073,11 +1073,11 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] loiter at current altitude"); + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; @@ -1175,14 +1175,14 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); } } @@ -1331,7 +1331,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1357,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1384,7 +1384,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1516,7 +1516,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: [navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", _mission_item.time_inside); } } @@ -1541,7 +1541,7 @@ Navigator::on_mission_item_reached() if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: [navigator] takeoff completed"); + mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); } else { /* advance by one mission item */ -- cgit v1.2.3 From 73edc020162841f250cf8ba26c6bdc40837a0e4f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Feb 2014 13:42:15 +0100 Subject: 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 42504dea9..15f2fd2ca 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); } -- cgit v1.2.3 From ff8f92af31fe1eadb52dbe1e612fd1f31b00ff82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:24:03 -0800 Subject: Remove Ubuntu code style script - people should use the reference one --- Tools/fix_code_style_ubuntu.sh | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100755 Tools/fix_code_style_ubuntu.sh diff --git a/Tools/fix_code_style_ubuntu.sh b/Tools/fix_code_style_ubuntu.sh deleted file mode 100755 index 90ab57b89..000000000 --- a/Tools/fix_code_style_ubuntu.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh -astyle \ - --style=linux \ - --indent=force-tab=8 \ - --indent-cases \ - --indent-preprocessor \ - --break-blocks=all \ - --pad-oper \ - --pad-header \ - --unpad-paren \ - --keep-one-line-blocks \ - --keep-one-line-statements \ - --align-pointer=name \ - --suffix=none \ - --lineend=linux \ - $* - #--ignore-exclude-errors-x \ - #--exclude=EASTL \ - #--align-reference=name \ -- cgit v1.2.3 From 99b376089f6a35c977174ae8e01bc4686b405cc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:26:23 -0800 Subject: Replace single if lines with brackets in astyle --- Tools/fix_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index 832ee79da..0b6743013 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -16,4 +16,5 @@ astyle \ --ignore-exclude-errors-x \ --lineend=linux \ --exclude=EASTL \ + --add-brackets \ $* -- cgit v1.2.3 From 20618a611b5cd719cab803823b2f3a08d9a498f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:44:05 -0800 Subject: Fixed a number of compile warnings in mount tests. --- src/systemcmds/tests/test_mount.c | 38 +++++++++++++++++--------------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 44e34d9ef..4b6303cfb 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -141,8 +141,8 @@ test_mount(int argc, char *argv[]) /* 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); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); } @@ -162,7 +162,7 @@ test_mount(int argc, char *argv[]) } char buf[64]; - int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + (void)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); @@ -174,8 +174,8 @@ test_mount(int argc, char *argv[]) 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); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); for (unsigned a = 0; a < alignments; a++) { @@ -185,22 +185,20 @@ test_mount(int argc, char *argv[]) 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++) { + for (unsigned 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]) { + if (wret != (int)chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) @@ -214,8 +212,8 @@ test_mount(int argc, char *argv[]) fsync(fd); } else { printf("#"); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); } } @@ -224,12 +222,10 @@ test_mount(int argc, char *argv[]) } printf("."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(200000); - end = hrt_absolute_time(); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -237,7 +233,7 @@ test_mount(int argc, char *argv[]) for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, chunk_sizes[c]); - if (rret != chunk_sizes[c]) { + if (rret != (int)chunk_sizes[c]) { warnx("READ ERROR!"); return 1; } @@ -245,7 +241,7 @@ test_mount(int argc, char *argv[]) /* compare value */ bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { + for (unsigned 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; @@ -271,16 +267,16 @@ test_mount(int argc, char *argv[]) } } - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(20000); /* we always reboot for the next test if we get here */ warnx("Iteration done, rebooting.."); - fsync(stdout); - fsync(stderr); + fsync(fileno(stdout)); + fsync(fileno(stderr)); usleep(50000); systemreset(false); -- cgit v1.2.3 From e1f60e770948d28ec1152f66b9c3ad57eae0c2e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 12:53:20 -0800 Subject: Code style fixes (authors should be in the doxygen section), using lseek to attempt to work around log corruption --- src/modules/sdlog2/logbuffer.c | 3 +-- src/modules/sdlog2/sdlog2.c | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index b3243f7b5..6a29d7e5c 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c4fafb5a6..16c37459b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2012-2014 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 @@ -95,6 +93,7 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ + } else { \ log_msgs_skipped++; \ } @@ -450,6 +449,7 @@ static void *logwriter_thread(void *arg) n = available; } + lseek(log_fd, 0, SEEK_CUR); n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; -- cgit v1.2.3 From ab8ece3961f50348cf4bf464cb7d953b182a0a44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 12:42:48 +0100 Subject: l1_pos_control: prevent NaNs for roll setpoint --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 3b68a0a4e..d1c864d78 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -38,6 +38,8 @@ * */ +#include + #include "ecl_l1_pos_controller.h" float ECL_L1_Pos_Controller::nav_roll() @@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* calculate the vector from waypoint A to current 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::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } /* calculate eta angle towards the loiter center */ -- cgit v1.2.3 From 75c6706278119e50ce56ecbb1620025a9b9b936d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:19:57 +0100 Subject: l1_pos_control: set gndspeed_undershoot to 0 in loiter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ed57e0d34..2e8d038d0 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 @@ -695,7 +695,7 @@ void 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) { + if (_global_pos_valid && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); -- cgit v1.2.3 From 909e138182000350df19d47367d6f68d0a6b7fd4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 14:27:24 +0100 Subject: l1_pos_control: fix stupid 'if not' mistake --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2e8d038d0..ed6d8792c 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 @@ -695,7 +695,7 @@ void 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 && !pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); -- cgit v1.2.3 From 926c4701c71fb2689025decbc454d14c6df85e76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:17:13 +0100 Subject: 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 15f2fd2ca..1ce467a7b 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 5559d7b56..4d135a0ef 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 fdc4ffff6..72dddebfe 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 845c16583..2bd4da82e 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: -- cgit v1.2.3 From ba0bf456b9b0cf6cdc58ce2fd80192da3c1744d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:37:35 +0100 Subject: gitignore: ignore pydev project file created in eclipse --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 3e94cf620..71326517f 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/ /Documentation/doxygen*objdb*tmp .tags .tags_sorted_by_file +.pydevproject -- cgit v1.2.3 From 5ed5e04cb24c261c9a8b40857bd1d733238f847e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:43:23 +0100 Subject: sdlog2: code style fixes broke compilation --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 16c37459b..41e2248bb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -93,7 +93,6 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ - } else { \ log_msgs_skipped++; \ } -- cgit v1.2.3 From d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Feb 2014 23:46:58 +0400 Subject: mavlink: major rewrite, prepare for dynamic mavlink streams configuration, WIP --- src/modules/mavlink/mavlink_orb_listener.cpp | 1196 ++++++++++------------ src/modules/mavlink/mavlink_orb_listener.h | 97 +- src/modules/mavlink/mavlink_orb_subscription.cpp | 47 + src/modules/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, 721 insertions(+), 735 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 fdc196371..ea1e3a8bb 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) -{ - 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) +MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval) { - /* 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); + /* 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; } } - 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; - - /* 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); - } + MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval); - l->listener->gps_counter++; + stream->mavlink = _mavlink; + + 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; - - /* - * Initialise listener array. - * - * Might want to invoke each listener once to set initial state. - */ - struct pollfd fds[_max_listeners]; + /* add mavlink streams */ + /* common buffer for topics and data sizes */ + const struct orb_metadata *topics[1]; + size_t sizes[1]; - 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 26a2e832f..1a103e43d 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); - - 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 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 new file mode 100644 index 000000000..182407a5e --- /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 000000000..2c72abaef --- /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 000000000..2a6728fdb --- /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 000000000..0f959d720 --- /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 76798eb12..dc8d5586f 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 -- cgit v1.2.3 From a79eef05bcb23c9be386a2980437a67151cfa3ea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Feb 2014 23:48:00 +0400 Subject: perf_counter: added include --- src/modules/systemlib/perf_counter.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index e1e3cbe95..d8f69fdbf 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -39,6 +39,8 @@ #ifndef _SYSTEMLIB_PERF_COUNTER_H #define _SYSTEMLIB_PERF_COUNTER_H value +#include + /** * Counter types. */ -- cgit v1.2.3 From cf7ac7e660b4be82d98b881fea6cbc718978605b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 25 Feb 2014 00:04:44 +0400 Subject: 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 182407a5e..16044ef72 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; } -- cgit v1.2.3 From 4e27fd9a381bd32ba5b79d275528ac19d1fb9442 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:22:19 +0400 Subject: 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 b60a7e0b9..2144d3460 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,6 +8,8 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ +#include + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, -- cgit v1.2.3 From e291af990fd9a4f447cbad2416b78d031cd33f5c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:24:14 +0400 Subject: 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 | 436 +++++---------- src/modules/mavlink/mavlink_main.h | 88 +-- src/modules/mavlink/mavlink_messages.cpp | 613 ++++++++++++++++++++ src/modules/mavlink/mavlink_messages.h | 28 + src/modules/mavlink/mavlink_orb_listener.cpp | 679 ----------------------- src/modules/mavlink/mavlink_orb_listener.h | 138 ----- src/modules/mavlink/mavlink_orb_subscription.cpp | 23 +- src/modules/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 +- 11 files changed, 822 insertions(+), 1209 deletions(-) create mode 100644 src/modules/mavlink/mavlink_messages.cpp create mode 100644 src/modules/mavlink/mavlink_messages.h delete mode 100644 src/modules/mavlink/mavlink_orb_listener.cpp 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 1ce467a7b..4a75b00ab 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(); - /* 1 Hz */ - if (lowspeed_counter % 10 == 0) { + if (param_sub->update(t)) { + /* parameters updated */ 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,26 +1651,27 @@ Mavlink::task_main(int argc, char *argv[]) /* 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()); - /* 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()); + mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { - mavlink_pm_queued_send(); - } + 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); + /* 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); } @@ -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 9461a51f8..56d262000 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; - }; - - struct mavlink_subscriptions subs; + MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - 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_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp new file mode 100644 index 000000000..77ec344dc --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -0,0 +1,613 @@ +/* + * mavlink_messages.cpp + * + * Created on: 25.02.2014 + * Author: ton + */ + +#include + +#include +#include + +#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; + } + + /* 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; + } + + /* 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 +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 +//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_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); +// +//} diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h new file mode 100644 index 000000000..78279c08f --- /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.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp deleted file mode 100644 index ea1e3a8bb..000000000 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ /dev/null @@ -1,679 +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 orb_listener.cpp - * Monitors ORB topics and sends update messages as appropriate. - * - * @author Lorenz Meier - * @author Julian Oes - */ - -// 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 "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) -{ - if (m < 0.0f) { - return 0; - - } else if (m > 655.35f) { - return 65535; - } - - return (uint16_t)(m * 100.0f); -} - -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; - } - } - - /* 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::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) -{ - MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval); - - stream->mavlink = _mavlink; - - 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::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) -//{ -// /* 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 */ - 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_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h deleted file mode 100644 index 1a103e43d..000000000 --- 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 16044ef72..84ac11483 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 2c72abaef..9a7340e9b 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 2a6728fdb..9df4263ee 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 0f959d720..c3e60917e 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 dc8d5586f..5ea6c0f46 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 -- cgit v1.2.3 From 769a2af1f8925a2d47fd47a2d25f8d7baac150ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:38:21 +0400 Subject: 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 4a75b00ab..1563a257b 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 77ec344dc..7a56c36a5 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 78279c08f..a6326bad1 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_ */ -- cgit v1.2.3 From 7310fd608500be69153c5d033f74b056f1bb986e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 21:28:35 +0400 Subject: 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 | 442 ++++++++++++++--------- src/modules/mavlink/mavlink_messages.h | 15 +- src/modules/mavlink/mavlink_orb_subscription.cpp | 36 +- src/modules/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 | 25 +- 9 files changed, 443 insertions(+), 361 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1563a257b..33d81729c 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 56d262000..e7f3486da 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 7a56c36a5..df73581f0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -12,195 +12,289 @@ #include "mavlink_messages.h" +class MavlinkStreamHeartbeat : public MavlinkStream { +public: + const char *get_name() + { + return "HEARTBEAT"; + } -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 } -}; + MavlinkStream *new_instance() + { + return new MavlinkStreamHeartbeat(); + } -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; +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; + struct vehicle_status_s *status; + struct position_setpoint_triplet_s *pos_sp_triplet; - /* HIL */ - if (status->hil_state == HIL_STATE_ON) { - mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } +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(); - /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + 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(); } - /* 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) { + 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; + + /* 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); + } - 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 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); -} - -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); -} + 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 -//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 a6326bad1..3dc6cb699 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 84ac11483..b504b6955 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 9a7340e9b..c38a9cc43 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 d07de0f22..b828420e6 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 9df4263ee..16407366e 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 c3e60917e..9f175adbe 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 { +private: + hrt_abstime _last_sent; + +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + 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; - 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(); + 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; }; -- cgit v1.2.3 From 2967763b160bb12b8e13e72219fcb689da015ab5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:13:49 +0400 Subject: 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 df73581f0..0880e8285 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); } -- cgit v1.2.3 From f9619e39341dbc45683c6ccd7e06397f8a537216 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:46:33 +0400 Subject: 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 9b3e202e6..59c04f0e3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -441,14 +441,14 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; - while (bearing > M_PI_F) { + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; if (c++ > 3) return NAN; } c = 0; - while (bearing <= -M_PI_F) { + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; if (c++ > 3) return NAN; @@ -465,14 +465,14 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; - while (bearing > M_TWOPI_F) { + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; if (c++ > 3) return NAN; } c = 0; - while (bearing <= 0.0f) { + while (bearing < 0.0f) { bearing += M_TWOPI_F; if (c++ > 3) return NAN; @@ -489,14 +489,14 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; - while (bearing > 180.0f) { + while (bearing >= 180.0f) { bearing -= 360.0f; if (c++ > 3) return NAN; } c = 0; - while (bearing <= -180.0f) { + while (bearing < -180.0f) { bearing += 360.0f; if (c++ > 3) return NAN; @@ -513,14 +513,14 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; - while (bearing > 360.0f) { + while (bearing >= 360.0f) { bearing -= 360.0f; if (c++ > 3) return NAN; } c = 0; - while (bearing <= 0.0f) { + while (bearing < 0.0f) { bearing += 360.0f; if (c++ > 3) return NAN; -- cgit v1.2.3 From 85dc422d9804c894009e37c6eaab67a10c5dca28 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:47:19 +0400 Subject: 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 33d81729c..5ac76e1cf 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 0880e8285..27ae197a6 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) -- cgit v1.2.3 From 18f72f8dd7a3bd4a9af00fe28afd29e0cc65e2e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 23:02:53 +0400 Subject: 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 5ac76e1cf..3102c937d 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 27ae197a6..48443fbdc 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); -- cgit v1.2.3 From 141982a3ac21e7a0437f1d7692e4024daf873c21 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 13:54:55 +0400 Subject: 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 +++++++++++++---------- src/modules/mavlink/mavlink_orb_subscription.cpp | 2 +- src/modules/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 3102c937d..8a026742c 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 e7f3486da..506b4317a 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 48443fbdc..967606841 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 b504b6955..35470a19a 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 c38a9cc43..79ff3abdb 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 000000000..f5bb06ccd --- /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 000000000..6db65f638 --- /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 16407366e..703f74b4c 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 5ea6c0f46..f09efa2e6 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 -- cgit v1.2.3 From 2159f948ea81088076b440cd8b673b1ac9f70da3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:31:41 +0400 Subject: 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 3d8be089e..558be4275 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 76f021e33..c3065b6fc 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 8a026742c..974fd27bf 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 (Mavlink::instance_exists(device_name, this)) { - errx(1, "mavlink instance for %s already running", device_name); + if (_datarate == 0) { + /* convert bits to bytes and use 1/2 of bandwidth by default */ + _datarate = _baudrate / 20; } - struct termios uart_config_original; + if (_datarate > MAX_DATA_RATE) { + _datarate = MAX_DATA_RATE; + } - bool usb_uart; + if (Mavlink::instance_exists(device_name, this)) { + errx(1, "mavlink instance for %s already running", device_name); + } /* 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 506b4317a..afbf85787 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 b828420e6..a3546e954 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 -- cgit v1.2.3 From f8187650083b361f61c07ceb712c21a25d1c80c4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:32:55 +0400 Subject: top: CPU % indication bug fixed --- src/systemcmds/top/top.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 1ca3fc928..37e913040 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -233,8 +233,8 @@ top_main(void) system_load.tasks[i].tcb->pid, CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100), - (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), + (int)(curr_loads[i] * 100.0f), + (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), stack_size - stack_free, stack_size, system_load.tasks[i].tcb->sched_priority, -- cgit v1.2.3 From efca2d158aa34c912be91fb3229b211022ae0945 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 00:45:59 +0400 Subject: 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 974fd27bf..672daf641 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 afbf85787..049f5fedd 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); -- cgit v1.2.3 From 8783bed114e8ece85d62f15d0d2a06c28aa0bcfe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 00:49:12 +0400 Subject: sdlog2: getopt invalid usage fixed --- src/modules/sdlog2/sdlog2.c | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 68e6a7469..f952c3f32 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -652,6 +652,10 @@ int sdlog2_thread_main(int argc, char *argv[]) argv += 2; int ch; + /* 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:b:eat")) != EOF) { switch (ch) { case 'r': { @@ -699,13 +703,20 @@ int sdlog2_thread_main(int argc, char *argv[]) } else { warnx("unknown option character `\\x%x'", optopt); } + err_flag = true; + break; default: - sdlog2_usage("unrecognized flag"); - errx(1, "exiting"); + warnx("unrecognized flag"); + err_flag = true; + break; } } + if (err_flag) { + sdlog2_usage(NULL); + } + gps_time = 0; /* create log root dir */ -- cgit v1.2.3 From 967f81bfabbab0773fa29c079e36678670bcf67a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 23:43:52 +0400 Subject: mavlink_onboard removed --- makefiles/config_px4fmu-v2_default.mk | 1 - src/modules/mavlink_onboard/mavlink.c | 537 --------------------- .../mavlink_onboard/mavlink_bridge_header.h | 83 ---- src/modules/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 2ad2f63a9..412d0c9e1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,6 @@ MODULES += systemcmds/hw_ver MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink -MODULES += modules/mavlink_onboard # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c deleted file mode 100644 index ab9ce45f3..000000000 --- a/src/modules/mavlink_onboard/mavlink.c +++ /dev/null @@ -1,537 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 b72bbb2b1..000000000 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 4658bcc1d..000000000 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 a7a4980fa..000000000 --- a/src/modules/mavlink_onboard/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# MAVLink protocol to uORB interface process (XXX hack for onboard use) -# - -MODULE_COMMAND = mavlink_onboard -SRCS = mavlink.c \ - mavlink_receiver.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h deleted file mode 100644 index bbc9f6e66..000000000 --- a/src/modules/mavlink_onboard/orb_topics.h +++ /dev/null @@ -1,102 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 c84b6fd26..000000000 --- a/src/modules/mavlink_onboard/util.h +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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); -- cgit v1.2.3 From 323b90bfd9f3a1524f36c089d532ed3836cc2ea3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 23:44:51 +0400 Subject: 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 049f5fedd..41e781ee8 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 967606841..3634acefa 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) -- cgit v1.2.3 From 77d1989abae9d267bb74f86fb0104dbefcbcd52a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:06:30 +0400 Subject: 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 3634acefa..8097ecdb3 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,73 +1027,13 @@ 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) //{ // orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); @@ -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) //{ // -- cgit v1.2.3 From 836f7c435fe31572e45333877142dce8b4d2fc78 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:16:51 +0400 Subject: 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 +- src/modules/mavlink/mavlink_orb_subscription.cpp | 44 +- src/modules/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 672daf641..c97786553 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; - Mavlink* inst = ::_head; - while (inst != nullptr) { + while (inst != nullptr) { - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; - inst = inst->next; + 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 41e781ee8..532c9bcee 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 8097ecdb3..820faae1c 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 3dc6cb699..b8823263a 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 35470a19a..e1208bca9 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 79ff3abdb..3cf33ccef 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 f5bb06ccd..f6ed6e662 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 6db65f638..0b37538e6 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 a3546e954..f85773ae0 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 fca5de917..199e42689 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 703f74b4c..869495098 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 9f175adbe..135e1bce0 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; -- cgit v1.2.3 From f2b46389ee8c8e9dd73ad9ac1fc8170c759e8b1c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 28 Feb 2014 21:42:41 +0100 Subject: fw: reset landing and takeoff state if switched to manual --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 55 ++++++++++++++++++---- 1 file changed, 45 insertions(+), 10 deletions(-) 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 a4142266b..ab2a2439b 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 @@ -176,6 +176,8 @@ private: bool launch_detected; bool usePreTakeoffThrust; + bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) + /* Landingslope object */ Landingslope landingslope; @@ -346,6 +348,16 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + /* + * Reset takeoff state + */ + int reset_takeoff_state(); + + /* + * Reset landing state + */ + int reset_landing_state(); }; namespace l1_control @@ -396,7 +408,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false) + usePreTakeoffThrust(false), + last_manual(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -1042,20 +1055,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // mission is active _loiter_hold = false; - /* reset land state */ + /* reset landing state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { - land_noreturn_horizontal = false; - land_noreturn_vertical = false; - land_stayonground = false; - land_motor_lim = false; - land_onslope = false; + reset_landing_state(); } /* reset takeoff/launch state */ if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { - launch_detected = false; - usePreTakeoffThrust = false; - launchDetector.reset(); + reset_takeoff_state(); } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1150,6 +1157,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; + + /* reset landing and takeoff state */ + if (!last_manual) { + reset_landing_state(); + reset_takeoff_state(); + } } if (usePreTakeoffThrust) { @@ -1160,6 +1173,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _att_sp.pitch_body = _tecs.get_pitch_demand(); + if (_control_mode.flag_control_position_enabled) { + last_manual = false; + } else { + last_manual = true; + } + return setpoint; } @@ -1310,6 +1329,22 @@ FixedwingPositionControl::task_main() _exit(0); } +int FixedwingPositionControl::reset_takeoff_state() +{ + launch_detected = false; + usePreTakeoffThrust = false; + launchDetector.reset(); +} + +int FixedwingPositionControl::reset_landing_state() +{ + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; +} + int FixedwingPositionControl::start() { -- cgit v1.2.3 From eac00a71ea8f0b10d446be752f6fef737f850956 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 11:05:07 +0100 Subject: navigator: don't enter AUTO_READY for fixedwing because we don't have a land detector yet --- src/modules/navigator/navigator_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 04f87d4ca..8d0710e91 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1386,7 +1386,8 @@ Navigator::set_rtl_item() void Navigator::request_loiter_or_ready() { - if (_vstatus.condition_landed) { + /* XXX workaround: no landing detector for fixedwing yet */ + if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { dispatch(EVENT_READY_REQUESTED); } else { -- cgit v1.2.3 From 1b8004cd8ecf7824584aac9e7fed447714feb716 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 16:43:04 +0400 Subject: 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 ++++++++-------- src/modules/mavlink/mavlink_orb_subscription.cpp | 9 +-- src/modules/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 c3065b6fc..17f7dd077 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 c97786553..b996413a8 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 532c9bcee..ebea53d52 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 820faae1c..8be7d76d7 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 e1208bca9..6279e5366 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 3cf33ccef..eacc27034 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; -- cgit v1.2.3 From c10ef787539265bc36fb76c855aa19b30ea24b04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 17:12:46 +0400 Subject: 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 b996413a8..568666c1e 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 f85773ae0..b6e008cbf 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; -- cgit v1.2.3 From 256cc2b411b1f36397884bfd019b9ac3e4cd1850 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 18:30:30 +0400 Subject: 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 17f7dd077..c96f44e02 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 568666c1e..99bef1f73 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 ebea53d52..94bef7e72 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; /** -- cgit v1.2.3 From b264352c179dafd42066ca8ffbdf855c7f111207 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2014 15:47:09 +0100 Subject: startup: phantom script was missing the default line --- ROMFS/px4fmu_common/init.d/3031_phantom | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 2e2434bb8..a3004d1e1 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -5,4 +5,6 @@ # Simon Wilks # +sh /etc/init.d/rc.fw_defaults + set MIXER FMU_Q -- cgit v1.2.3 From 6948defdb26711fd8336b5e0173664bf98517406 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 19:27:53 +0400 Subject: 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 558be4275..0de2d4295 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 c96f44e02..48532e7af 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 99bef1f73..fb29c9c71 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 94bef7e72..b52c12796 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; -- cgit v1.2.3 From a1ea89ea2d3665709f5249ebd7cc3fbadaca603a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 19:42:29 +0400 Subject: 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 fb29c9c71..e6c474aa7 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 f6ed6e662..9cdda8b17 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 869495098..bb19d7e33 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; } /** -- cgit v1.2.3 From 85001b52aeddaab0f5a1faea8eade8294d447e9f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 20:09:02 +0400 Subject: 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 e6c474aa7..c10c95dbd 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); -- cgit v1.2.3 From 63bdb749adc26550b0cfb6dd8b4f0e0a7173ba10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:41:27 +0400 Subject: 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 b6e008cbf..032958b74 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" -- cgit v1.2.3 From e06263f76f9e38ea9624f1f1ab21af8731a3cd7f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:43:56 +0400 Subject: position_estimator_inav: failsafe against NaN estimate --- .../position_estimator_inav/position_estimator_inav_main.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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 ad363efe0..fb68dd31c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -708,6 +708,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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); + float x_est_prev[3], y_est_prev[3]; + + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -715,7 +720,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ @@ -739,7 +745,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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; + memcpy(x_est, x_est_prev, sizeof(x_est)); + memcpy(y_est, y_est_prev, sizeof(y_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_flow, 0, sizeof(corr_flow)); } } -- cgit v1.2.3 From d11235585cf1014c5391b63e394151c171ed286d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:56:02 +0400 Subject: position_estimator_inav: log writing on NaN estimate fixed --- .../position_estimator_inav/position_estimator_inav_main.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 fb68dd31c..d6d03367b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], 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); + unsigned n = 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]); + fwrite(s, 1, n, f); + n = 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); + fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); } -- cgit v1.2.3 From 624ff010185c16dffacc8d000aae2748ff1cd0a1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 12:53:46 +0400 Subject: mavlink: HIL fixes, send 0 when disarmed --- src/modules/mavlink/mavlink_messages.cpp | 92 +++++++++++++------------------- 1 file changed, 36 insertions(+), 56 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8be7d76d7..c22289772 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); - - } 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); + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + } else { + out[i] = -1.0f; + } } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } }; -- cgit v1.2.3 From db157b68c2f3dba4209f8075f6ae7ae9e81a448c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 19:42:10 +0400 Subject: mc_pos_control: remove some debug logs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b4bac53d6..78d06ba5b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -791,7 +791,6 @@ MulticopterPositionControl::task_main() } thrust_int(2) = -i; - mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } } else { @@ -803,7 +802,6 @@ MulticopterPositionControl::task_main() 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 { -- cgit v1.2.3 From cb8095c5ac932bbf49137491d92b4cf064021058 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 19:56:37 +0400 Subject: 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 ff1932c66..c45cafc1b 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); -- cgit v1.2.3 From 3674aae4a746099a162cbf943311b587cd46822f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 21:19:56 +0400 Subject: commander: ignore battery voltage in HIL mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5af63120..483437029 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -962,7 +962,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* 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) { + if (status.hil_state == HIL_STATE_OFF && 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; -- cgit v1.2.3 From b76e26c5e5f37c4fb086a68e0427d9d297e8d225 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 21:24:59 +0400 Subject: commander: allow arming with safety enabled in HIL mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 483437029..d114a2e5c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); -- cgit v1.2.3 From 3107f4d62cb07de70619093be57ce2b634763eba Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:26:26 +0400 Subject: mavlink: UART receiver major cleanup --- src/modules/mavlink/mavlink_receiver.cpp | 812 ++++++++++++++++--------------- src/modules/mavlink/mavlink_receiver.h | 57 +-- 2 files changed, 438 insertions(+), 431 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 032958b74..8a8027738 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,287 +338,366 @@ 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); } - } - - /* - * Only decode hil messages in HIL mode. - * - * The HIL mode is enabled by the HIL bit flag - * in the system mode. Either send a set mode - * COMMAND_LONG message or a SET_MODE message - */ - - if (_mavlink->get_hil_enabled()) { - - uint64_t timestamp = hrt_absolute_time(); - - if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); - - /* sensors general */ - hil_sensors.timestamp = hrt_absolute_time(); - - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; - hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; - hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = hil_counter; - - /* accelerometer */ - static const float mg2ms2 = 9.8f / 1000.0f; - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = hil_counter; - - /* adc */ - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - - /* magnetometer */ - float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; - hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; - hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; - hil_sensors.magnetometer_ga[0] = imu.xmag; - hil_sensors.magnetometer_ga[1] = imu.ymag; - hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = hil_counter; - - /* baro */ - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = hil_counter; - /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; + } else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + /* manual control */ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); - /* airspeed from differential pressure, ambient pressure and temp */ - struct airspeed_s airspeed; + /* rc channels */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); - float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); - // XXX need to fix this - float tas = ias; + rc.timestamp = timestamp; + rc.chan_count = 4; - airspeed.timestamp = hrt_absolute_time(); - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; + 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 (pub_hil_airspeed < 0) { - pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + if (_rc_pub == 0) { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { - orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); } + } + + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); + /* 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); - /* 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(); + 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 (pub_hil_gyro < 0) { - pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { - orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); } + } + } - struct accel_report accel; - - accel.x_raw = imu.xacc / mg2ms2; - - accel.y_raw = imu.yacc / mg2ms2; - - accel.z_raw = imu.zacc / mg2ms2; - - accel.x = imu.xacc; + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + */ - accel.y = imu.yacc; + 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); - accel.z = imu.zacc; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - accel.temperature = imu.temperature; + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; - accel.timestamp = hrt_absolute_time(); + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } } - struct mag_report mag; + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); - mag.x_raw = imu.xmag / mga2ga; + 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; - mag.y_raw = imu.ymag / mga2ga; + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - mag.z_raw = imu.zmag / mga2ga; + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } - mag.x = imu.xmag; + /* 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); - mag.y = imu.ymag; + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } - mag.z = imu.zmag; + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); - mag.timestamp = hrt_absolute_time(); + 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 (pub_hil_mag < 0) { - pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - } else { - orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } } - struct baro_report baro; - - baro.pressure = imu.abs_pressure; - - baro.altitude = imu.pressure_alt; - - baro.temperature = imu.temperature; + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); - baro.timestamp = hrt_absolute_time(); + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; - if (pub_hil_baro < 0) { - pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - } else { - orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } } - /* publish combined sensor topic */ - if (pub_hil_sensors > 0) { - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + /* 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 { - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } else { + _sensors_pub = 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; + /* 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); + + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } - // increment counters - hil_counter++; - hil_frames++; + /* 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; + /* 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; } - } - - 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.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.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; - /* 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; - 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); - } + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + hil_local_pos.landed = landed; - /* 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(); + if (_local_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); - /* 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; - - 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.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); - } - - /* 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); + /* 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 { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } } - } - - 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); + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - 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; + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - /* fake RC channels with manual control input from simulator */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - - 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 199e42689..b97919e9d 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; }; -- cgit v1.2.3 From 14ddf1804aa9d698724fda62288b210fd850d2ac Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:48:20 +0400 Subject: 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 c22289772..d4c77d1fd 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; } -- cgit v1.2.3 From 5707118a976e63a810203b3e9ad6e74837ab7f30 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 3 Mar 2014 22:09:01 +0100 Subject: add simple trim parameter for fw attitude --- src/modules/fw_att_control/fw_att_control_main.cpp | 35 +++++++++++++++++----- 1 file changed, 28 insertions(+), 7 deletions(-) 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 17b1028f9..b040fb5df 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -166,6 +166,11 @@ private: float airspeed_min; float airspeed_trim; float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -197,6 +202,10 @@ private: param_t airspeed_min; param_t airspeed_trim; param_t airspeed_max; + + param_t trim_roll; + param_t trim_pitch; + param_t trim_yaw; } _parameter_handles; /**< handles for interesting parameters */ @@ -335,6 +344,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); + _parameter_handles.trim_roll = param_find("TRIM_ROLL"); + _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); + _parameter_handles.trim_yaw = param_find("TRIM_YAW"); + /* fetch initial parameter values */ parameters_update(); } @@ -395,6 +408,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); + param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); + param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -670,9 +687,13 @@ FixedwingAttitudeControl::task_main() * With this mapping the stick angle is a 1:1 representation of * the commanded attitude. If more than 45 degrees are desired, * a scaling parameter can be applied to the remote. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. */ - roll_sp = _manual.roll * 0.75f; - pitch_sp = _manual.pitch * 0.75f; + roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f; + pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -685,7 +706,7 @@ FixedwingAttitudeControl::task_main() att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = roll_sp; att_sp.pitch_body = pitch_sp; - att_sp.yaw_body = 0.0f; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ @@ -719,12 +740,12 @@ FixedwingAttitudeControl::task_main() speed_body_u, speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude - /* Run attitude RATE controllers which need the desired attitudes from above */ + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { warnx("roll_u %.4f", roll_u); } @@ -733,7 +754,7 @@ FixedwingAttitudeControl::task_main() _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); @@ -743,7 +764,7 @@ FixedwingAttitudeControl::task_main() _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { warnx("yaw_u %.4f", yaw_u); } -- cgit v1.2.3 From 190eb6205dc3e610d223878c4b85a8e587fc6323 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 11:45:58 +0400 Subject: 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 d4c77d1fd..7475160d5 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) //{ // -- cgit v1.2.3 From a7012a54180386b5dc7f86fe6a1564690ecc8240 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Mar 2014 08:34:55 +1100 Subject: tests: added "tests file2" filesystem test this is useful for testing for filesystem corruption on cluster boundaries --- src/systemcmds/tests/module.mk | 1 + src/systemcmds/tests/test_file2.c | 196 ++++++++++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 199 insertions(+) create mode 100644 src/systemcmds/tests/test_file2.c diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index acf28c35b..622a0faf3 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -26,6 +26,7 @@ SRCS = test_adc.c \ test_mixer.cpp \ test_mathlib.cpp \ test_file.c \ + test_file2.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c new file mode 100644 index 000000000..ef555f6c3 --- /dev/null +++ b/src/systemcmds/tests/test_file2.c @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * 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 test_file2.c + * + * File write test. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define FLAG_FSYNC 1 +#define FLAG_LSEEK 2 + +/* + return a predictable value for any file offset to allow detection of corruption + */ +static uint8_t get_value(uint32_t ofs) +{ + union { + uint32_t ofs; + uint8_t buf[4]; + } u; + u.ofs = ofs; + return u.buf[ofs % 4]; +} + +static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags) +{ + printf("Testing on %s with write_chunk=%u write_size=%u\n", + filename, (unsigned)write_chunk, (unsigned)write_size); + + uint32_t ofs = 0; + int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC); + if (fd == -1) { + perror(filename); + exit(1); + } + + // create a file of size write_size, in write_chunk blocks + uint8_t counter = 0; + while (ofs < write_size) { + uint8_t buffer[write_chunk]; + for (uint16_t j=0; j 0) { + filename = argv[0]; + } + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + test_corruption(filename, write_chunk, write_size, flags); + return 0; +} + diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index ac64ad33d..ad55e1410 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -107,6 +107,7 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_file2(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[]); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 73827b7cf..fe22f6177 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -104,6 +104,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"file2", test_file2, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, -- cgit v1.2.3 From 2ec4ee6fc08f5368a52028de83f420ffeb249698 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 12:33:03 +0400 Subject: mavlink_receiver: split message handlers to separate methods --- src/modules/mavlink/mavlink_receiver.cpp | 1199 ++++++++++++++++-------------- src/modules/mavlink/mavlink_receiver.h | 11 + 2 files changed, 642 insertions(+), 568 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8a8027738..ef1a747da 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,681 +121,743 @@ 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)); - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; - - /* check if topic is advertised */ - if (_cmd_pub <= 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - /* publish */ - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - } - } + case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: + handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); + break; - } else if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status(msg); + break; - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; - 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; + default: + break; + } - if (_flow_pub <= 0) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + */ + if (_mavlink->get_hil_enabled()) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; + + default: + break; } + } +} - } 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); +void +MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; } else { - /* create command */ - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* check if topic is advertised */ + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } } + } +} + +void +MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub <= 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } +} - } 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); +void +MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) +{ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; + /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = custom_mode.main_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = VEHICLE_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } +} - struct vehicle_vicon_position_s vicon_position; - memset(&vicon_position, 0, sizeof(vicon_position)); - vicon_position.timestamp = timestamp; +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); - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.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 (_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); - } + } 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); +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); - if (mavlink_system.sysid < 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + 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); + uint8_t ml_mode = 0; + bool ml_armed = false; - /* - * rate control mode - defined by MAVLink - */ + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + break; - uint8_t ml_mode = 0; - bool ml_armed = false; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; + break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; - break; + break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; - break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + ml_armed = false; + } - 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; + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = static_cast(ml_mode); - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } + offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast(ml_mode); + if (_offboard_control_sp_pub <= 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - offboard_control_sp.timestamp = timestamp; + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + } +} - if (_offboard_control_sp_pub <= 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &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); + } +} - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); - } +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); } + } - } else if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { - /* telemetry status */ - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + /* 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); - 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; + 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 (_telemetry_status_pub <= 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); } + } +} - } else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - /* manual control */ - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); +void +MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); + uint64_t timestamp = hrt_absolute_time(); - rc.timestamp = timestamp; - rc.chan_count = 4; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - 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; + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; - if (_rc_pub == 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } + 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)); - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); + 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; - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - 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; + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } - if (_manual_pub == 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } + 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); } } - /* - * Only decode hil messages in HIL mode. - * - * The HIL mode is enabled by the HIL bit flag - * in the system mode. Either send a set mode - * COMMAND_LONG message or a SET_MODE message - */ + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); - 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); + 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; - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } - airspeed.timestamp = timestamp; - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); - if (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - /* 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); - } - } + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } - /* 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); - } - } + /* 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); - /* magnetometer */ - { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); + } else { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } + } - mag.timestamp = timestamp; - mag.x_raw = imu.xmag * 1000.0f; - mag.y_raw = imu.ymag * 1000.0f; - mag.z_raw = imu.zmag * 1000.0f; - mag.x = imu.xmag; - mag.y = imu.ymag; - mag.z = imu.zmag; + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - } + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - /* baro */ - { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + } - baro.timestamp = timestamp; - baro.pressure = imu.abs_pressure; - baro.altitude = imu.pressure_alt; - baro.temperature = imu.temperature; + /* increment counters */ + _hil_counter++; + _hil_frames++; - if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + /* 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 { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } - } +void +MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) +{ + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); - /* sensor combined */ - { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); - - hil_sensors.timestamp = timestamp; - - hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; - hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; - hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = _hil_counter; - - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = _hil_counter; - - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - - hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; - hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; - hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; - hil_sensors.magnetometer_ga[0] = imu.xmag; - hil_sensors.magnetometer_ga[1] = imu.ymag; - hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = _hil_counter; - - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = _hil_counter; - - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = _hil_counter; - - /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); - - } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - } - } + uint64_t timestamp = hrt_absolute_time(); - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; + hil_gps.timestamp_time = timestamp; + hil_gps.time_gps_usec = gps.time_usec; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + 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 - } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } + 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; - /* increment counters */ - _hil_counter++; - _hil_frames++; + 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); - /* 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; - } + hil_gps.timestamp_satellites = timestamp; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; - } else if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - /* HIL GPS */ - mavlink_hil_gps_t gps; - mavlink_msg_hil_gps_decode(msg, &gps); + if (_gps_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); - struct vehicle_gps_position_s hil_gps; - memset(&hil_gps, 0, sizeof(hil_gps)); + } else { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } +} - hil_gps.timestamp_time = timestamp; - hil_gps.time_gps_usec = gps.time_usec; +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); - 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 + uint64_t timestamp = hrt_absolute_time(); - hil_gps.timestamp_variance = timestamp; - hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - 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); + 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; - hil_gps.timestamp_satellites = timestamp; - hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } - } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - } + /* 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 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); + } else { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - 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; + 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 (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + if (_global_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } + } else { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } + } - /* 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); - } - } + /* 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; + } - /* 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); - } - } + 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); - /* 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; - } + } else { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + } + } - 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)); - /* 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); - } - } + 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)); + /* 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; + 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); + 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); - } - } + } 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 b97919e9d..beaae2058 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; -- cgit v1.2.3 From e1742eea7d951aa704665c2b5db4d1c6788aafa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 13:03:17 +0400 Subject: 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 7475160d5..4d83afe82 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); } }; -- cgit v1.2.3 From ea70e967565a15ffc06aeb95df87f47440df97c1 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 4 Mar 2014 16:16:39 +0100 Subject: Added ARDrone to autostart --- ROMFS/px4fmu_common/init.d/rc.autostart | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 3968af58e..7aaf7133e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -106,6 +106,15 @@ then sh /etc/init.d/4001_quad_x fi +# +# ARDrone +# + +if param compare SYS_AUTOSTART 4008 8 +then + sh /etc/init.d/4008_ardrone +fi + if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 -- cgit v1.2.3 From 958f7597e7d13f82e41b9c8e0aa4c3ac552cdf1c Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 4 Mar 2014 16:33:18 +0100 Subject: Added actual ARDrone start up script. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 40 +++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4008_ardrone diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone new file mode 100644 index 000000000..fa9b9d18a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -0,0 +1,40 @@ +#!nsh +# +# ARDrone +# + +# Just use the default multicopter settings. +sh /etc/init.d/rc.mc_defaults + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 + + param set SYS_AUTOCONFIG 0 + param save +fi + +set FMU_MODE gpio + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +set MAV_TYPE 2 +set VEHICLE_TYPE mc +param set BAT_V_SCALING 0.008381 -- cgit v1.2.3 From 9817922bf96611f59552b6a6fd2aebab790135f5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Mar 2014 20:34:20 +1100 Subject: FMUv2: switch debug console and 2nd GPS this allows a normal GPS cable to be used for the 2nd GPS, making dual-GPS setups easier --- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2a734c27e..26222c255 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set -# CONFIG_UART7_RXDMA is not set +CONFIG_UART7_RXDMA=y # CONFIG_UART8_RS485 is not set CONFIG_UART8_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y @@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # CONFIG_USART3_SERIAL_CONSOLE is not set # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set -# CONFIG_UART7_SERIAL_CONSOLE is not set -CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART7_SERIAL_CONSOLE=y +# CONFIG_UART8_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set # -- cgit v1.2.3 From 046def571d21856f24f6afc805450c003858afea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 23:11:24 +0400 Subject: sensors: publish manual controls on actuator_controls_1 topic instead of actuator_controls_3 --- src/modules/sensors/sensors.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417..068fb3d0b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -195,7 +195,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ - orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ + orb_advert_t _actuator_group_1_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -459,7 +459,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), - _actuator_group_3_pub(-1), + _actuator_group_1_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -1279,7 +1279,7 @@ Sensors::rc_poll() return; struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; + struct actuator_controls_s actuator_group_1; /* initialize to default values */ manual_control.roll = NAN; @@ -1460,14 +1460,14 @@ Sensors::rc_poll() } /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; + actuator_group_1.control[0] = manual_control.roll; + actuator_group_1.control[1] = manual_control.pitch; + actuator_group_1.control[2] = manual_control.yaw; + actuator_group_1.control[3] = manual_control.throttle; + actuator_group_1.control[4] = manual_control.flaps; + actuator_group_1.control[5] = manual_control.aux1; + actuator_group_1.control[6] = manual_control.aux2; + actuator_group_1.control[7] = manual_control.aux3; /* check if ready for publishing */ if (_rc_pub > 0) { @@ -1487,11 +1487,11 @@ Sensors::rc_poll() } /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + if (_actuator_group_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuator_group_1_pub, &actuator_group_1); } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + _actuator_group_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_group_1); } } -- cgit v1.2.3 From a50957c3cec92167594b7bd82561f72100ccebdd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 23:13:20 +0400 Subject: px4io: print all control groups in "px4io status" --- src/drivers/px4io/px4io.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7c7b3dcb7..4c48194d4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1921,12 +1921,14 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("controls"); + for (unsigned group = 0; group < PX4IO_CONTROL_GROUPS; group++) { + printf("controls %u:", group); - for (unsigned i = 0; i < _max_controls; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); - printf("\n"); + printf("\n"); + } for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; -- cgit v1.2.3 From 529013ae1c9298c44e808a8fa97336b242623ad8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 00:00:17 +0400 Subject: Reverse: sensors: publish manual controls on actuator_controls_1 topic instead of actuator_controls_3 --- src/modules/sensors/sensors.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 068fb3d0b..b176d7417 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -195,7 +195,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ - orb_advert_t _actuator_group_1_pub; /**< manual control as actuator topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -459,7 +459,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), - _actuator_group_1_pub(-1), + _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -1279,7 +1279,7 @@ Sensors::rc_poll() return; struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_1; + struct actuator_controls_s actuator_group_3; /* initialize to default values */ manual_control.roll = NAN; @@ -1460,14 +1460,14 @@ Sensors::rc_poll() } /* copy from mapped manual control to control group 3 */ - actuator_group_1.control[0] = manual_control.roll; - actuator_group_1.control[1] = manual_control.pitch; - actuator_group_1.control[2] = manual_control.yaw; - actuator_group_1.control[3] = manual_control.throttle; - actuator_group_1.control[4] = manual_control.flaps; - actuator_group_1.control[5] = manual_control.aux1; - actuator_group_1.control[6] = manual_control.aux2; - actuator_group_1.control[7] = manual_control.aux3; + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; /* check if ready for publishing */ if (_rc_pub > 0) { @@ -1487,11 +1487,11 @@ Sensors::rc_poll() } /* check if ready for publishing */ - if (_actuator_group_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuator_group_1_pub, &actuator_group_1); + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); } else { - _actuator_group_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_group_1); + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } } -- cgit v1.2.3 From af548db7cc0a53b1c07a74342ec528e6247b0fcb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 00:01:27 +0400 Subject: px4iofirmware: define 4 actuator control groups --- src/modules/px4iofirmware/mixer.cpp | 2 +- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6a4429461..9558198f3 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -267,7 +267,7 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group > 3) + if (control_group >= PX4IO_CONTROL_GROUPS) return -1; switch (source) { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bb224f388..54c5663a5 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -53,7 +53,7 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_CONTROL_GROUPS 2 +#define PX4IO_CONTROL_GROUPS 4 #define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ -- cgit v1.2.3 From 149bf4582c67296097106566ec98a85229be9509 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 00:05:17 +0400 Subject: px4io: hardcode number of control groups in "px4io status" --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4c48194d4..136a6e29e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1921,7 +1921,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - for (unsigned group = 0; group < PX4IO_CONTROL_GROUPS; group++) { + for (unsigned group = 0; group < 4; group++) { printf("controls %u:", group); for (unsigned i = 0; i < _max_controls; i++) -- cgit v1.2.3 From 3639c7ae91f5226b71cdba8d8cc98eae4bba7e07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Mar 2014 21:36:09 +0100 Subject: Fixed warning --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6a4429461..dfa98d425 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -327,7 +327,7 @@ mixer_handle_text(const void *buffer, size_t length) return 1; } - px4io_mixdata *msg = (px4io_mixdata *)buffer; + px4io_mixdata *msg = static_cast(buffer); isr_debug(2, "mix txt %u", length); -- cgit v1.2.3 From fabef6c889f0fc347e0059ee6f8ede83bf0f4f32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Mar 2014 22:52:34 +0100 Subject: Revert "Fixed warning" This reverts commit 3639c7ae91f5226b71cdba8d8cc98eae4bba7e07. --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index dfa98d425..6a4429461 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -327,7 +327,7 @@ mixer_handle_text(const void *buffer, size_t length) return 1; } - px4io_mixdata *msg = static_cast(buffer); + px4io_mixdata *msg = (px4io_mixdata *)buffer; isr_debug(2, "mix txt %u", length); -- cgit v1.2.3 From 87fd89ea48dab591efd38c083e356d49b369bce9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 5 Mar 2014 00:23:05 +0100 Subject: fw pitch sp and roll sp offset parameter --- src/modules/fw_att_control/fw_att_control_main.cpp | 21 +++++++++++++++------ src/modules/fw_att_control/fw_att_control_params.c | 10 ++++++++++ 2 files changed, 25 insertions(+), 6 deletions(-) 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 b040fb5df..ab6c1415a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -170,6 +170,8 @@ private: float trim_roll; float trim_pitch; float trim_yaw; + float rollsp_offset; + float pitchsp_offset; } _parameters; /**< local copies of interesting parameters */ @@ -206,6 +208,8 @@ private: param_t trim_roll; param_t trim_pitch; param_t trim_yaw; + param_t rollsp_offset; + param_t pitchsp_offset; } _parameter_handles; /**< handles for interesting parameters */ @@ -347,6 +351,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.trim_roll = param_find("TRIM_ROLL"); _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); + _parameter_handles.rollsp_offset = param_find("FW_RSP_OFF"); + _parameter_handles.pitchsp_offset = param_find("FW_PSP_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -411,6 +417,9 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); + param_get(_parameter_handles.rollsp_offset, &(_parameters.rollsp_offset)); + param_get(_parameter_handles.pitchsp_offset, &(_parameters.pitchsp_offset)); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); @@ -665,13 +674,13 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp = 0.0f; - float pitch_sp = 0.0f; + float roll_sp = _parameters.rollsp_offset; + float pitch_sp = _parameters.pitchsp_offset; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { - roll_sp = _att_sp.roll_body; - pitch_sp = _att_sp.pitch_body; + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -692,8 +701,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f; + roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset; + pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 1c615094c..c80a44f2a 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); // @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively // @Range 0.0 to 30 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); + +// @DisplayName Roll Setpoint Offset +// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe +// @Range -90.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); + +// @DisplayName Pitch Setpoint Offset +// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe +// @Range -90.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -- cgit v1.2.3 From 1d9d24f605aeb13b87efe2f6cf82232768b148ac Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 10:50:16 +0100 Subject: Added flag for ARDrone interface. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 14 +++++--------- ROMFS/px4fmu_common/init.d/rcS | 12 +++++++++++- src/drivers/px4fmu/fmu.cpp | 2 +- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index fa9b9d18a..d6b2319f9 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -3,6 +3,8 @@ # ARDrone # +echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" + # Just use the default multicopter settings. sh /etc/init.d/rc.mc_defaults @@ -29,12 +31,6 @@ then param save fi -set FMU_MODE gpio - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -set MAV_TYPE 2 -set VEHICLE_TYPE mc -param set BAT_V_SCALING 0.008381 +set FMU_MODE gpio_serial +set OUTPUT_MODE fmu +set ARDRONE yes \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 76f021e33..af78162ed 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -397,8 +397,10 @@ then set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - mavlink start + mavlink start -d /dev/ttyS0 usleep 5000 + + set EXIT_ON_END yes fi fi @@ -427,6 +429,14 @@ then gps start fi + # + # Start up ARDrone Motor interface + # + if [ $ARDRONE == yes ] + then + ardrone_interface start -d /dev/ttyS1 + fi + # # Fixed wing setup # diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0fbd84924..a70ff6c5c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[]) } - fprintf(stderr, "FMU: unrecognised command, try:\n"); + fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) -- cgit v1.2.3 From 9ffa5a665a2ac8b887cbd9383eab1947137f0def Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 10:56:58 +0100 Subject: Set the mavlink port settings back to ttyS1 by default. --- ROMFS/px4fmu_common/init.d/rcS | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index af78162ed..1baac0eb3 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes ] + if [ $TTYS1_BUSY == yes -o $ARDRONE == yes] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 @@ -397,10 +397,8 @@ then set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - mavlink start -d /dev/ttyS0 + mavlink start usleep 5000 - - set EXIT_ON_END yes fi fi -- cgit v1.2.3 From 5976815f2baff6b17ca41dbb28e3e5698c12a180 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 15:07:05 +0100 Subject: Fixed startup parameters. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 29 ++++++++++++++--------------- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index d6b2319f9..d38796cca 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -11,24 +11,23 @@ sh /etc/init.d/rc.mc_defaults # # 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 MC_ATTRATE_D 0 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0 - param set MC_ATT_I 0.3 - param set MC_ATT_P 5 - param set MC_YAWPOS_D 0.1 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 1 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 + param set MC_ROLL_P 5.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.0 + param set MC_PITCH_P 5.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.0 + param set MC_YAW_P 1.0 + param set MC_YAW_D 0.1 param set MC_YAWRATE_P 0.15 - - param set SYS_AUTOCONFIG 0 - param save + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.15 fi set FMU_MODE gpio_serial diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1baac0eb3..9983aa0e7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes -o $ARDRONE == yes] + if [ $TTYS1_BUSY == yes -o $ARDRONE == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 -- cgit v1.2.3 From 30665816f0005cfb32b048a9ad4db84f78b3eff3 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 15:11:41 +0100 Subject: Added a newline at the end of file. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index d38796cca..cb8260ccc 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -32,4 +32,4 @@ fi set FMU_MODE gpio_serial set OUTPUT_MODE fmu -set ARDRONE yes \ No newline at end of file +set ARDRONE yes -- cgit v1.2.3 From bcdc8c9e1dcefc2176fd1f853d4bae90e7d196d6 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 5 Mar 2014 16:16:10 +0100 Subject: Get rid of one set of beeps. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index cb8260ccc..512c4cb73 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -33,3 +33,4 @@ fi set FMU_MODE gpio_serial set OUTPUT_MODE fmu set ARDRONE yes +set USE_IO no -- cgit v1.2.3 From a8ff126dfe214245d1f5860f2bc9c17b6cdac34b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 20:00:13 +0400 Subject: px4io: print actuator control registers as int16 instead of uint16 --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 136a6e29e..8e990aa6f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1925,7 +1925,7 @@ PX4IO::print_status() printf("controls %u:", group); for (unsigned i = 0; i < _max_controls; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); + printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); printf("\n"); } -- cgit v1.2.3 From 36ba60d63c024c23efdb4bba780ac98b5b257ee6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 20:03:21 +0400 Subject: mc_att_control: hotfix, memset all structs to zeroes on start, fixes garbage in actuator controls 4..8 --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 24226880e..9cb8e8344 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -262,8 +262,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : { memset(&_v_att, 0, sizeof(_v_att)); memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); _params.att_p.zero(); -- cgit v1.2.3 From b40d3e1e18bd8032eee97abdd9a4f68d87303f65 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 22:30:43 +0400 Subject: 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 7aa71f182..dab62df5f 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 d42b23ab0..e560c805a 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 -- cgit v1.2.3 From 8425b9bef21e310d1cbd29aad65d34e9dd974d55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 22:46:22 +0400 Subject: 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 dab62df5f..8cb873640 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 e560c805a..0fceb2043 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 -- cgit v1.2.3 From f60a8af30eee42e5f0c3cea996f32b14fab0f1db Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 5 Mar 2014 22:17:00 +0100 Subject: fw sp offsets: convert deg to rad --- src/modules/fw_att_control/fw_att_control_main.cpp | 32 ++++++++++++---------- 1 file changed, 18 insertions(+), 14 deletions(-) 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 ab6c1415a..5ade835ff 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -170,8 +170,10 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset; - float pitchsp_offset; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -208,8 +210,8 @@ private: param_t trim_roll; param_t trim_pitch; param_t trim_yaw; - param_t rollsp_offset; - param_t pitchsp_offset; + param_t rollsp_offset_deg; + param_t pitchsp_offset_deg; } _parameter_handles; /**< handles for interesting parameters */ @@ -351,8 +353,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.trim_roll = param_find("TRIM_ROLL"); _parameter_handles.trim_pitch = param_find("TRIM_PITCH"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); - _parameter_handles.rollsp_offset = param_find("FW_RSP_OFF"); - _parameter_handles.pitchsp_offset = param_find("FW_PSP_OFF"); + _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); + _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -417,8 +419,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll)); param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch)); param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw)); - param_get(_parameter_handles.rollsp_offset, &(_parameters.rollsp_offset)); - param_get(_parameter_handles.pitchsp_offset, &(_parameters.pitchsp_offset)); + param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg)); + param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); + _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); + _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); /* pitch control parameters */ @@ -674,13 +678,13 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp = _parameters.rollsp_offset; - float pitch_sp = _parameters.pitchsp_offset; + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset; + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -701,8 +705,8 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset; + roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; + pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; -- cgit v1.2.3 From a897c97d955de4873aea1f01f65cab11faab2d3a Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 6 Mar 2014 12:45:10 +0100 Subject: Changed ARDRONE to an OUTPUT_MODE setting and added a skip option to mixer. Fewer beeps than before. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 5 ++--- ROMFS/px4fmu_common/init.d/rc.interface | 9 ++++++--- ROMFS/px4fmu_common/init.d/rcS | 13 +++++++++---- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 512c4cb73..39fe66234 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -30,7 +30,6 @@ then param set MC_YAW_FF 0.15 fi -set FMU_MODE gpio_serial -set OUTPUT_MODE fmu -set ARDRONE yes +set OUTPUT_MODE ardrone set USE_IO no +set MIXER skip \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d25f01dde..afe71460d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -3,7 +3,7 @@ # Script to configure control interface # -if [ $MIXER != none ] +if [ $MIXER != none -a $MIXER != skip] then # # Load mixer @@ -33,8 +33,11 @@ then tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] Mixer not defined" - tone_alarm $TUNE_OUT_ERROR + if [ $MIXER != skip ] + then + echo "[init] Mixer not defined" + tone_alarm $TUNE_OUT_ERROR + fi fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 9983aa0e7..7b9ae0995 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -240,6 +240,11 @@ then fi fi + if [ $OUTPUT_MODE == ardrone ] + then + set FMU_MODE gpio_serial + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -277,7 +282,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - if [ $OUTPUT_MODE == fmu ] + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ] then echo "[init] Use FMU PWM as primary output" if fmu mode_$FMU_MODE @@ -351,7 +356,7 @@ then fi fi else - if [ $OUTPUT_MODE != fmu ] + if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ] then if fmu mode_$FMU_MODE then @@ -387,7 +392,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes -o $ARDRONE == yes ] + if [ $TTYS1_BUSY == yes -o $OUTPUT_MODE == ardrone ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 @@ -430,7 +435,7 @@ then # # Start up ARDrone Motor interface # - if [ $ARDRONE == yes ] + if [ $OUTPUT_MODE == ardrone ] then ardrone_interface start -d /dev/ttyS1 fi -- cgit v1.2.3 From 25faf1b8f8716323233d6f966aac65e0c9e8d61a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Mar 2014 17:44:10 +0400 Subject: ardrone: minor fixes --- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 +- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 39fe66234..0f98f7b6c 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -32,4 +32,4 @@ fi set OUTPUT_MODE ardrone set USE_IO no -set MIXER skip \ No newline at end of file +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7b9ae0995..b1cd919f7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -284,7 +284,7 @@ then fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ] then - echo "[init] Use FMU PWM as primary output" + echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -299,7 +299,7 @@ then then set TTYS1_BUSY yes fi - if [ $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ] then set TTYS1_BUSY yes fi @@ -372,7 +372,7 @@ then then set TTYS1_BUSY yes fi - if [ $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ] then set TTYS1_BUSY yes fi @@ -392,7 +392,7 @@ then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $TTYS1_BUSY == yes -o $OUTPUT_MODE == ardrone ] + if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 -- cgit v1.2.3 From 61eb228e4d8cfc95c1c4a84ed870f2b4918d7c6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Mar 2014 15:17:53 +0100 Subject: Reduce excessive stack sizes on main OS stacks. This has been tested on mavlink_beta, but needs more careful testing. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 1dc96b3c3..20edc68aa 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50 # # Stack and heap information # -CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_IDLETHREAD_STACKSIZE=4096 CONFIG_USERMAIN_STACKSIZE=4096 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -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=2048 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set -- cgit v1.2.3 From 2fbcc32870f59aa1f08337ca99129b35902971be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 6 Mar 2014 23:00:00 +0100 Subject: fix comparison in rcS --- 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 b1cd919f7..57299d772 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -282,7 +282,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ] + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE -- cgit v1.2.3 From 602e9c87062409955c122631d273c6aae50331ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 6 Mar 2014 23:11:13 +0100 Subject: fix if in rc.interface --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index afe71460d..7f793b219 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -3,7 +3,7 @@ # Script to configure control interface # -if [ $MIXER != none -a $MIXER != skip] +if [ $MIXER != none -a $MIXER != skip ] then # # Load mixer -- cgit v1.2.3 From e505f4fae5596b2b53c120a7cb2a03d2d974c83a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 11:17:02 +0400 Subject: sdlog2: use orb_check() instead of poll() to minimize polling overhead, bugs and compiler warnings fixed --- src/modules/sdlog2/sdlog2.c | 838 ++++++++++++++++++-------------------------- 1 file changed, 333 insertions(+), 505 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e70..fa25d74f8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -159,6 +159,8 @@ static void *logwriter_thread(void *arg); */ __EXPORT int sdlog2_main(int argc, char *argv[]); +static bool copy_if_updated(orb_id_t topic, int handle, void *buffer); + /** * Mainloop of sd log deamon. */ @@ -402,7 +404,7 @@ static void *logwriter_thread(void *arg) int log_fd = open_log_file(); if (log_fd < 0) - return; + return NULL; struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; @@ -483,7 +485,7 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return; + return NULL; } void sdlog2_start_log() @@ -628,6 +630,19 @@ int write_parameters(int fd) return written; } +bool copy_if_updated(orb_id_t topic, int handle, void *buffer) +{ + bool updated; + + orb_check(handle, &updated); + + if (updated) { + orb_copy(topic, handle, buffer); + } + + return updated; +} + int sdlog2_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -636,9 +651,8 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first"); } - /* log buffer size */ + useconds_t sleep_delay = 10000; // default log rate: 100 Hz int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; - logging_enabled = false; log_on_start = false; log_when_armed = false; @@ -656,12 +670,10 @@ int sdlog2_thread_main(int argc, char *argv[]) case 'r': { unsigned long r = strtoul(optarg, NULL, 10); - if (r == 0) { - sleep_delay = 0; - - } else { - sleep_delay = 1000000 / r; + if (r <= 0) { + r = 1; } + sleep_delay = 1000000 / r; } break; @@ -711,7 +723,7 @@ int sdlog2_thread_main(int argc, char *argv[]) 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); + err(1, "failed creating log root dir: %s", log_root); } /* copy conversion scripts */ @@ -733,8 +745,10 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + struct vehicle_gps_position_s buf_gps_pos; memset(&buf_status, 0, sizeof(buf_status)); + memset(&buf_gps_pos, 0, sizeof(buf_gps_pos)); /* warning! using union here to save memory, elements should be used separately! */ union { @@ -749,7 +763,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; 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; struct rc_channels_s rc; @@ -763,30 +776,6 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); - struct { - int cmd_sub; - int status_sub; - int sensor_sub; - int att_sub; - int att_sp_sub; - int rates_sp_sub; - int act_outputs_sub; - int act_controls_sub; - int local_pos_sub; - int local_pos_sp_sub; - int global_pos_sub; - int triplet_sub; - int gps_pos_sub; - int vicon_pos_sub; - int flow_sub; - int rc_sub; - int airspeed_sub; - int esc_sub; - int global_vel_sp_sub; - int battery_sub; - int telemetry_sub; - } subs; - /* log message buffer: header + body */ #pragma pack(push, 1) struct { @@ -821,154 +810,51 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* 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]; + struct { + int cmd_sub; + int status_sub; + int sensor_sub; + int att_sub; + int att_sp_sub; + int rates_sp_sub; + int act_outputs_sub; + int act_controls_sub; + int local_pos_sub; + int local_pos_sp_sub; + int global_pos_sub; + int triplet_sub; + int gps_pos_sub; + int vicon_pos_sub; + int flow_sub; + int rc_sub; + int airspeed_sub; + int esc_sub; + int global_vel_sp_sub; + int battery_sub; + int telemetry_sub; + } subs; - /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds[fdsc_count].fd = subs.cmd_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); - fds[fdsc_count].fd = subs.status_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS POSITION --- */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_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; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE --- */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - fds[fdsc_count].fd = subs.att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT --- */ subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - fds[fdsc_count].fd = subs.att_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RATES SETPOINT --- */ subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - fds[fdsc_count].fd = subs.rates_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR OUTPUTS --- */ subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); - fds[fdsc_count].fd = subs.act_outputs_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL --- */ subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.act_controls_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - fds[fdsc_count].fd = subs.local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION SETPOINT --- */ subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - fds[fdsc_count].fd = subs.local_pos_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION --- */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - fds[fdsc_count].fd = subs.global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION SETPOINT--- */ subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - fds[fdsc_count].fd = subs.triplet_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VICON POSITION --- */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - fds[fdsc_count].fd = subs.vicon_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); - fds[fdsc_count].fd = subs.flow_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RC CHANNELS --- */ subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); - fds[fdsc_count].fd = subs.rc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); - fds[fdsc_count].fd = subs.esc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL VELOCITY SETPOINT --- */ subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); - fds[fdsc_count].fd = subs.global_vel_sp_sub; - 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++; - - /* --- TELEMETRY STATUS --- */ subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - fds[fdsc_count].fd = subs.telemetry_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__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms - */ - const int poll_timeout = 1000; thread_running = true; @@ -990,8 +876,8 @@ int sdlog2_thread_main(int argc, char *argv[]) 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; + if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + gps_time = buf_gps_pos.time_gps_usec; } } @@ -999,390 +885,334 @@ int sdlog2_thread_main(int argc, char *argv[]) } 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 3) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ + if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) { + handle_command(&buf.cmd); + } - /* handle the poll result */ - if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging"); - main_thread_should_exit = true; + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (status_updated) { + if (log_when_armed) { + handle_status(&buf_status); + } + } - } else if (poll_ret > 0) { + /* --- GPS POSITION - LOG MANAGEMENT --- */ + bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); + if (gps_pos_updated && log_name_timestamp) { + gps_time = buf_gps_pos.time_gps_usec; + } - /* check all data subscriptions only if logging enabled, - * logging_enabled can be changed while checking vehicle_command and vehicle_status */ - bool check_data = logging_enabled; - int ifds = 0; - int handled_topics = 0; + if (!logging_enabled) { + continue; + } - /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + pthread_mutex_lock(&logbuffer_mutex); - handle_command(&buf.cmd); - handled_topics++; - } + /* write time stamp message */ + log_msg.msg_type = LOG_TIME_MSG; + log_msg.body.log_TIME.t = hrt_absolute_time(); + LOGBUFFER_WRITE_AND_COUNT(TIME); + + /* --- VEHICLE STATUS --- */ + if (status_updated) { + log_msg.msg_type = LOG_STAT_MSG; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + 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; + LOGBUFFER_WRITE_AND_COUNT(STAT); + } - /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + /* --- GPS POSITION --- */ + if (gps_pos_updated) { + log_msg.msg_type = LOG_GPS_MSG; + log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; + log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; + log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; + log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.lat = buf_gps_pos.lat; + log_msg.body.log_GPS.lon = buf_gps_pos.lon; + log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; + log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s; + log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; + log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; + log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; + LOGBUFFER_WRITE_AND_COUNT(GPS); + } - if (log_when_armed) { - handle_status(&buf_status); - } + /* --- SENSOR COMBINED --- */ + if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { + bool write_IMU = false; + bool write_SENS = false; - handled_topics++; + if (buf.sensor.gyro_counter != gyro_counter) { + gyro_counter = buf.sensor.gyro_counter; + write_IMU = true; } - /* --- 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 (buf.sensor.accelerometer_counter != accelerometer_counter) { + accelerometer_counter = buf.sensor.accelerometer_counter; + write_IMU = true; } - if (!logging_enabled || !check_data || handled_topics >= poll_ret) { - continue; + if (buf.sensor.magnetometer_counter != magnetometer_counter) { + magnetometer_counter = buf.sensor.magnetometer_counter; + write_IMU = true; } - ifds = 1; // begin from VEHICLE STATUS again - - pthread_mutex_lock(&logbuffer_mutex); - - /* write time stamp message */ - log_msg.msg_type = LOG_TIME_MSG; - log_msg.body.log_TIME.t = hrt_absolute_time(); - LOGBUFFER_WRITE_AND_COUNT(TIME); - - /* --- VEHICLE STATUS --- */ - if (fds[ifds++].revents & POLLIN) { - /* don't orb_copy, it's already done few lines above */ - log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - 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; - LOGBUFFER_WRITE_AND_COUNT(STAT); + if (buf.sensor.baro_counter != baro_counter) { + baro_counter = buf.sensor.baro_counter; + write_SENS = true; } - /* --- GPS POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* don't orb_copy, it's already done few lines above */ - log_msg.msg_type = LOG_GPS_MSG; - log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; - log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf.gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf.gps_pos.epv_m; - log_msg.body.log_GPS.lat = buf.gps_pos.lat; - log_msg.body.log_GPS.lon = buf.gps_pos.lon; - log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f; - log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; - log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; - log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; - log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad; - LOGBUFFER_WRITE_AND_COUNT(GPS); + if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { + differential_pressure_counter = buf.sensor.differential_pressure_counter; + write_SENS = true; } - /* --- SENSOR COMBINED --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - bool write_IMU = false; - bool write_SENS = false; - - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; - write_IMU = true; - } - - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; - write_IMU = true; - } - - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; - write_IMU = true; - } - - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; - write_SENS = true; - } - - if (write_IMU) { - log_msg.msg_type = LOG_IMU_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (write_SENS) { - log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } + if (write_IMU) { + log_msg.msg_type = LOG_IMU_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); } - /* --- ATTITUDE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - log_msg.msg_type = LOG_ATT_MSG; - log_msg.body.log_ATT.roll = buf.att.roll; - log_msg.body.log_ATT.pitch = buf.att.pitch; - log_msg.body.log_ATT.yaw = buf.att.yaw; - 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); + if (write_SENS) { + log_msg.msg_type = LOG_SENS_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + LOGBUFFER_WRITE_AND_COUNT(SENS); } + } - /* --- ATTITUDE SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); - log_msg.msg_type = LOG_ATSP_MSG; - log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; - log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; - log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; - log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; - LOGBUFFER_WRITE_AND_COUNT(ATSP); - } + /* --- ATTITUDE --- */ + if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { + log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.roll = buf.att.roll; + log_msg.body.log_ATT.pitch = buf.att.pitch; + log_msg.body.log_ATT.yaw = buf.att.yaw; + 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); + } - /* --- RATES SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp); - log_msg.msg_type = LOG_ARSP_MSG; - log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; - log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; - log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; - LOGBUFFER_WRITE_AND_COUNT(ARSP); - } + /* --- ATTITUDE SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) { + log_msg.msg_type = LOG_ATSP_MSG; + log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; + log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; + log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; + log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; + LOGBUFFER_WRITE_AND_COUNT(ATSP); + } - /* --- ACTUATOR OUTPUTS --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); - log_msg.msg_type = LOG_OUT0_MSG; - memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); - LOGBUFFER_WRITE_AND_COUNT(OUT0); - } + /* --- RATES SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) { + log_msg.msg_type = LOG_ARSP_MSG; + log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; + log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; + log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(ARSP); + } - /* --- ACTUATOR CONTROL --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); - log_msg.msg_type = LOG_ATTC_MSG; - log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; - log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; - log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; - log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; - LOGBUFFER_WRITE_AND_COUNT(ATTC); - } + /* --- ACTUATOR OUTPUTS --- */ + if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) { + log_msg.msg_type = LOG_OUT0_MSG; + memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT0); + } - /* --- LOCAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - log_msg.msg_type = LOG_LPOS_MSG; - log_msg.body.log_LPOS.x = buf.local_pos.x; - log_msg.body.log_LPOS.y = buf.local_pos.y; - log_msg.body.log_LPOS.z = buf.local_pos.z; - log_msg.body.log_LPOS.vx = buf.local_pos.vx; - log_msg.body.log_LPOS.vy = buf.local_pos.vy; - log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; - log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); - 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); - } - } + /* --- ACTUATOR CONTROL --- */ + if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) { + log_msg.msg_type = LOG_ATTC_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); + } - /* --- LOCAL POSITION SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp); - log_msg.msg_type = LOG_LPSP_MSG; - log_msg.body.log_LPSP.x = buf.local_pos_sp.x; - log_msg.body.log_LPSP.y = buf.local_pos_sp.y; - log_msg.body.log_LPSP.z = buf.local_pos_sp.z; - log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; - LOGBUFFER_WRITE_AND_COUNT(LPSP); + /* --- LOCAL POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { + log_msg.msg_type = LOG_LPOS_MSG; + log_msg.body.log_LPOS.x = buf.local_pos.x; + log_msg.body.log_LPOS.y = buf.local_pos.y; + log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.vx = buf.local_pos.vx; + log_msg.body.log_LPOS.vy = buf.local_pos.vy; + log_msg.body.log_LPOS.vz = buf.local_pos.vz; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); + 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; } - - /* --- GLOBAL POSITION --- */ - 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 * 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.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; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); - LOGBUFFER_WRITE_AND_COUNT(GPOS); + 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); } + } - /* --- GLOBAL POSITION SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; - 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.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - 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.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); - } + /* --- LOCAL POSITION SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { + log_msg.msg_type = LOG_LPSP_MSG; + log_msg.body.log_LPSP.x = buf.local_pos_sp.x; + log_msg.body.log_LPSP.y = buf.local_pos_sp.y; + log_msg.body.log_LPSP.z = buf.local_pos_sp.z; + log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(LPSP); + } - /* --- VICON POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - // TODO not implemented yet - } + /* --- GLOBAL POSITION --- */ + if (copy_if_updated(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 * 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.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; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + LOGBUFFER_WRITE_AND_COUNT(GPOS); + } - /* --- FLOW --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; - log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; - log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; - log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; - log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; - log_msg.body.log_FLOW.quality = buf.flow.quality; - log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; - LOGBUFFER_WRITE_AND_COUNT(FLOW); - } + /* --- GLOBAL POSITION SETPOINT --- */ + if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + 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.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + 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.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } - /* --- RC CHANNELS --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc); - log_msg.msg_type = LOG_RC_MSG; - /* Copy only the first 8 channels of 14 */ - memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); - log_msg.body.log_RC.channel_count = buf.rc.chan_count; - LOGBUFFER_WRITE_AND_COUNT(RC); - } + /* --- VICON POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { + // TODO not implemented yet + } - /* --- AIRSPEED --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); - log_msg.msg_type = LOG_AIRS_MSG; - log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; - log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; - LOGBUFFER_WRITE_AND_COUNT(AIRS); - } + /* --- FLOW --- */ + if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { + log_msg.msg_type = LOG_FLOW_MSG; + log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; + log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; + log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; + log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; + log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.quality = buf.flow.quality; + log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; + LOGBUFFER_WRITE_AND_COUNT(FLOW); + } - /* --- ESCs --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - - for (uint8_t i = 0; i < buf.esc.esc_count; i++) { - log_msg.msg_type = LOG_ESC_MSG; - log_msg.body.log_ESC.counter = buf.esc.counter; - log_msg.body.log_ESC.esc_count = buf.esc.esc_count; - log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; - log_msg.body.log_ESC.esc_num = i; - log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; - log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; - log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; - log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; - log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; - log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; - log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; - log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; - LOGBUFFER_WRITE_AND_COUNT(ESC); - } - } + /* --- RC CHANNELS --- */ + if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { + log_msg.msg_type = LOG_RC_MSG; + /* Copy only the first 8 channels of 14 */ + memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.chan_count; + LOGBUFFER_WRITE_AND_COUNT(RC); + } - /* --- GLOBAL VELOCITY SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); - log_msg.msg_type = LOG_GVSP_MSG; - log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; - log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; - log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; - LOGBUFFER_WRITE_AND_COUNT(GVSP); - } + /* --- AIRSPEED --- */ + if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) { + log_msg.msg_type = LOG_AIRS_MSG; + log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; + log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + LOGBUFFER_WRITE_AND_COUNT(AIRS); + } - /* --- 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); + /* --- ESCs --- */ + if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) { + for (uint8_t i = 0; i < buf.esc.esc_count; i++) { + log_msg.msg_type = LOG_ESC_MSG; + log_msg.body.log_ESC.counter = buf.esc.counter; + log_msg.body.log_ESC.esc_count = buf.esc.esc_count; + log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; + log_msg.body.log_ESC.esc_num = i; + log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; + log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; + log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; + log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; + log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; + log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; + log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; + log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; + LOGBUFFER_WRITE_AND_COUNT(ESC); } + } - /* --- TELEMETRY --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); - } + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) { + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } - /* 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 */ - pthread_cond_signal(&logbuffer_cond); - } + /* --- BATTERY --- */ + if (copy_if_updated(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); + } - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&logbuffer_mutex); + /* --- TELEMETRY --- */ + if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); } - if (use_sleep) { - usleep(sleep_delay); + /* 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 */ + pthread_cond_signal(&logbuffer_cond); } + + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&logbuffer_mutex); + + usleep(sleep_delay); } if (logging_enabled) @@ -1461,8 +1291,6 @@ int file_copy(const char *file_old, const char *file_new) void handle_command(struct vehicle_command_s *cmd) { - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; int param; /* request to set different system mode */ -- cgit v1.2.3 From 80595c177a65e9ba91a35ffdee49d518df657508 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 11:18:31 +0400 Subject: sdlog2: code style fixed --- src/modules/sdlog2/logbuffer.c | 3 ++- src/modules/sdlog2/sdlog2.c | 24 ++++++++++++++++++++---- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 6a29d7e5c..2da67d8a9 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; - if (available < 0) + if (available < 0) { available += lb->size; + } if (size > available) { // buffer overflow diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fa25d74f8..5acd2b615 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -222,8 +222,9 @@ static int open_log_file(void); static void sdlog2_usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" @@ -243,8 +244,9 @@ sdlog2_usage(const char *reason) */ int sdlog2_main(int argc, char *argv[]) { - if (argc < 2) + if (argc < 2) { sdlog2_usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -403,22 +405,29 @@ static void *logwriter_thread(void *arg) int log_fd = open_log_file(); - if (log_fd < 0) + if (log_fd < 0) { return NULL; + } struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; void *read_ptr; + int n = 0; + bool should_wait = false; + bool is_part = false; while (true) { @@ -673,6 +682,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (r <= 0) { r = 1; } + sleep_delay = 1000000 / r; } break; @@ -745,9 +755,11 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + struct vehicle_gps_position_s buf_gps_pos; memset(&buf_status, 0, sizeof(buf_status)); + memset(&buf_gps_pos, 0, sizeof(buf_gps_pos)); /* warning! using union here to save memory, elements should be used separately! */ @@ -892,6 +904,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (status_updated) { if (log_when_armed) { handle_status(&buf_status); @@ -900,6 +913,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - LOG MANAGEMENT --- */ bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); + if (gps_pos_updated && log_name_timestamp) { gps_time = buf_gps_pos.time_gps_usec; } @@ -1068,6 +1082,7 @@ int sdlog2_thread_main(int argc, char *argv[]) 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; @@ -1215,8 +1230,9 @@ int sdlog2_thread_main(int argc, char *argv[]) usleep(sleep_delay); } - if (logging_enabled) + if (logging_enabled) { sdlog2_stop_log(); + } pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 16bfc355d..e27518aa0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -267,13 +267,13 @@ struct log_DIST_s { /* --- TELE - TELEMETRY STATUS --- */ #define LOG_TELE_MSG 22 struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ -- cgit v1.2.3 From 2ee8214244efbe09981e71ea194573daa1c2f5bc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 11:27:24 +0400 Subject: sdlog2: move some global variables to local scope, set default log rate to 50 Hz --- src/modules/sdlog2/sdlog2.c | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5acd2b615..d784ecc4f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -134,12 +134,6 @@ static uint64_t gps_time = 0; /* current state of logging */ static bool logging_enabled = false; -/* enable logging on start (-e option) */ -static bool log_on_start = false; -/* enable logging when armed (-a option) */ -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; @@ -660,11 +654,14 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first"); } - useconds_t sleep_delay = 10000; // default log rate: 100 Hz + /* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */ + useconds_t sleep_delay = 20000; int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; logging_enabled = false; - log_on_start = false; - log_when_armed = false; + /* enable logging on start (-e option) */ + bool log_on_start = false; + /* enable logging when armed (-a option) */ + bool log_when_armed = false; log_name_timestamp = false; flag_system_armed = false; -- cgit v1.2.3 From c11e1ee0ab8579826ed73b1f596e0535156bbce4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:14:39 +0400 Subject: sdlog2: lseek() workaround removed --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d784ecc4f..200d70e93 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -454,7 +454,6 @@ static void *logwriter_thread(void *arg) n = available; } - lseek(log_fd, 0, SEEK_CUR); n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; -- cgit v1.2.3 From 2ee8cf1cf08095693b168238a311b1caf8df1701 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:28:38 +0400 Subject: sdlog2: bug fixed, sleep when idle too --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 200d70e93..58890322b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -893,6 +893,8 @@ int sdlog2_thread_main(int argc, char *argv[]) } while (!main_thread_should_exit) { + usleep(sleep_delay); + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) { handle_command(&buf.cmd); @@ -1222,8 +1224,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* unlock, now the writer thread may run */ pthread_mutex_unlock(&logbuffer_mutex); - - usleep(sleep_delay); } if (logging_enabled) { -- cgit v1.2.3 From a478dac621301a993fb8ed13b86abd61d93bfec0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:29:05 +0400 Subject: sdlog2: max write chunk increased to 1024 --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 58890322b..d21338376 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -110,7 +110,7 @@ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ 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 MAX_WRITE_CHUNK = 1024; static const int MIN_BYTES_TO_WRITE = 512; static const char *log_root = "/fs/microsd/log"; -- cgit v1.2.3 From aca6806b82b8ee201260a9e383879b6d821dba1a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:36:07 +0400 Subject: Revert "sdlog2: max write chunk increased to 1024" This reverts commit a478dac621301a993fb8ed13b86abd61d93bfec0. --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d21338376..58890322b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -110,7 +110,7 @@ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ 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 = 1024; +static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; static const char *log_root = "/fs/microsd/log"; -- cgit v1.2.3 From 39b1f7244ce06ff89492ad6843c5fc74a30dfe67 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 8 Mar 2014 11:37:49 +0400 Subject: 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 c10c95dbd..788fe5732 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; -- cgit v1.2.3 From cb8bd1a3ad21fec45af7a1f1d53fe7b891c59c2b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 8 Mar 2014 21:05:13 +0400 Subject: dumpfile command and fetch_log.py tool implemented to fetch logs via nsh console on USB --- Tools/fetch_log.py | 133 ++++++++++++++++++++++++++++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/systemcmds/dumpfile/dumpfile.c | 116 +++++++++++++++++++++++++++++ src/systemcmds/dumpfile/module.mk | 41 +++++++++++ 5 files changed, 292 insertions(+) create mode 100644 Tools/fetch_log.py create mode 100644 src/systemcmds/dumpfile/dumpfile.c create mode 100644 src/systemcmds/dumpfile/module.mk diff --git a/Tools/fetch_log.py b/Tools/fetch_log.py new file mode 100644 index 000000000..edcc6557c --- /dev/null +++ b/Tools/fetch_log.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# Log fetcher +# +# Print list of logs: +# python fetch_log.py +# +# Fetch log: +# python fetch_log.py sess001/log001.bin +# + +import serial, time, sys, os + +def wait_for_string(ser, s, timeout=1.0, debug=False): + t0 = time.time() + buf = [] + res = [] + n = 0 + while (True): + c = ser.read() + if debug: + sys.stderr.write(c) + buf.append(c) + if len(buf) > len(s): + res.append(buf.pop(0)) + n += 1 + if n % 10000 == 0: + sys.stderr.write(str(n) + "\n") + if "".join(buf) == s: + break + if timeout > 0.0 and time.time() - t0 > timeout: + raise Exception("Timeout while waiting for: " + s) + return "".join(res) + +def exec_cmd(ser, cmd, timeout): + ser.write(cmd + "\n") + ser.flush() + wait_for_string(ser, cmd + "\r\n", timeout) + return wait_for_string(ser, "nsh> \x1b[K", timeout) + +def ls_dir(ser, dir, timeout=1.0): + res = [] + for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]: + res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) + return res + +def list_logs(ser): + logs_dir = "/fs/microsd/log" + res = [] + for d in ls_dir(ser, logs_dir): + if d[2]: + sess_dir = d[0][:-1] + for f in ls_dir(ser, logs_dir + "/" + sess_dir): + log_file = f[0] + log_size = f[1] + res.append(sess_dir + "/" + log_file + "\t" + str(log_size)) + return "\n".join(res) + +def fetch_log(ser, fn, timeout): + cmd = "dumpfile " + fn + ser.write(cmd + "\n") + ser.flush() + wait_for_string(ser, cmd + "\r\n", timeout, True) + res = wait_for_string(ser, "\n", timeout, True) + data = [] + if res.startswith("OK"): + size = int(res.split()[1]) + n = 0 + print "Reading data:" + while (n < size): + buf = ser.read(min(size - n, 8192)) + data.append(buf) + n += len(buf) + sys.stdout.write(".") + sys.stdout.flush() + print + else: + raise Exception("Error reading log") + wait_for_string(ser, "nsh> \x1b[K", timeout) + return "".join(data) + +def main(): + dev = "/dev/tty.usbmodem1" + ser = serial.Serial(dev, "115200", timeout=0.2) + if len(sys.argv) < 2: + print list_logs(ser) + else: + log_file = sys.argv[1] + data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0) + try: + os.mkdir(log_file.split("/")[0]) + except: + pass + fout = open(log_file, "wb") + fout.write(data) + fout.close() + ser.close() + +if __name__ == "__main__": + main() diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index aff614cbb..651c2ac38 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -56,6 +56,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/hw_ver +MODULES += systemcmds/dumpfile # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..889abc4d9 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -63,6 +63,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver +MODULES += systemcmds/dumpfile # # General system control diff --git a/src/systemcmds/dumpfile/dumpfile.c b/src/systemcmds/dumpfile/dumpfile.c new file mode 100644 index 000000000..c18814342 --- /dev/null +++ b/src/systemcmds/dumpfile/dumpfile.c @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 dumpfile.c + * + * Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout. + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +__EXPORT int dumpfile_main(int argc, char *argv[]); + +int +dumpfile_main(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "usage: dumpfile "); + } + + /* open input file */ + FILE *f; + f = fopen(argv[1], "r"); + + if (f == NULL) { + printf("ERROR\n"); + exit(1); + } + + /* get file size */ + fseek(f, 0L, SEEK_END); + int size = ftell(f); + fseek(f, 0L, SEEK_SET); + + printf("OK %d\n", size); + + /* configure stdout */ + int out = fileno(stdout); + + struct termios tc; + struct termios tc_old; + tcgetattr(out, &tc); + + /* save old terminal attributes to restore it later on exit */ + memcpy(&tc_old, &tc, sizeof(tc)); + + /* don't add CR on each LF*/ + tc.c_oflag &= ~ONLCR; + + if (tcsetattr(out, TCSANOW, &tc) < 0) { + warnx("ERROR setting stdout attributes"); + exit(1); + } + + char buf[512]; + int nread; + + /* dump file */ + while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) { + if (write(out, buf, nread) <= 0) { + warnx("error dumping file"); + break; + } + } + + fsync(out); + fclose(f); + + /* restore old terminal attributes */ + if (tcsetattr(out, TCSANOW, &tc_old) < 0) { + warnx("ERROR restoring stdout attributes"); + exit(1); + } + + return OK; +} diff --git a/src/systemcmds/dumpfile/module.mk b/src/systemcmds/dumpfile/module.mk new file mode 100644 index 000000000..36461f477 --- /dev/null +++ b/src/systemcmds/dumpfile/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Dump file utility +# + +MODULE_COMMAND = dumpfile +SRCS = dumpfile.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 5c86eb6dd0be72cb3455a82020457d13515cb527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 18:48:24 +0100 Subject: Fix code style of mb12xx driver, no functional changes --- src/drivers/mb12xx/mb12xx.cpp | 237 ++++++++++++++++++++++++------------------ 1 file changed, 134 insertions(+), 103 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c910e2890..7693df295 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -37,7 +37,7 @@ * * Driver for the Maxbotix sonar range finders connected via I2C. */ - + #include #include @@ -84,7 +84,7 @@ /* Device limits */ #define MB12XX_MIN_DISTANCE (0.20f) #define MB12XX_MAX_DISTANCE (7.65f) - + #define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ @@ -102,17 +102,17 @@ class MB12XX : public device::I2C public: MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); virtual ~MB12XX(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -124,13 +124,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _range_finder_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -139,7 +139,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -147,12 +147,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Set the min and max distance thresholds if you want the end points of the sensors * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE @@ -162,7 +162,7 @@ private: void set_maximum_distance(float max); float get_minimum_distance(); float get_maximum_distance(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -177,8 +177,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ MB12XX::MB12XX(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -212,8 +212,9 @@ MB12XX::~MB12XX() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } int @@ -222,22 +223,25 @@ MB12XX::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* get a publish handle on the range finder topic */ struct range_finder_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); - if (_range_finder_topic < 0) + if (_range_finder_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -256,13 +260,13 @@ void MB12XX::set_minimum_distance(float min) { _min_distance = min; -} +} void MB12XX::set_maximum_distance(float max) { _max_distance = max; -} +} float MB12XX::get_minimum_distance() @@ -284,20 +288,20 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -307,13 +311,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -338,45 +345,49 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); - return -ENOMEM; + + return OK; } - irqrestore(flags); - - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - - case RANGEFINDERIOCSETMINIUMDISTANCE: - { - set_minimum_distance(*(float *)arg); - return 0; - } - break; - case RANGEFINDERIOCSETMAXIUMDISTANCE: - { - set_maximum_distance(*(float *)arg); - return 0; - } - break; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -391,8 +402,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -453,14 +465,14 @@ MB12XX::measure() uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); return ret; } + ret = OK; - + return ret; } @@ -468,32 +480,31 @@ int MB12XX::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[2] = {0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) - { + + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - + uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - + /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,17 +530,19 @@ MB12XX::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_RANGEFINDER}; + SUBSYSTEM_TYPE_RANGEFINDER + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -583,8 +596,9 @@ MB12XX::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -635,33 +649,37 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new MB12XX(MB12XX_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -674,15 +692,14 @@ fail: */ void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -700,22 +717,25 @@ test() int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("measurement: %0.2f m", (double)report.distance); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -726,14 +746,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("measurement: %0.3f", (double)report.distance); @@ -751,14 +773,17 @@ reset() { int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -769,8 +794,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -786,32 +812,37 @@ mb12xx_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { mb12xx::start(); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - mb12xx::stop(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + mb12xx::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { mb12xx::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { mb12xx::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { mb12xx::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } -- cgit v1.2.3 From 935362b7a369e75c18a7aed267d7b0b304e4c430 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 8 Mar 2014 19:00:49 +0100 Subject: launchdetection: better integration of Blocks class --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 17 +++++++++-------- src/lib/launchdetection/CatapultLaunchMethod.h | 11 ++++++++--- src/lib/launchdetection/LaunchDetector.cpp | 18 +++++++----------- src/lib/launchdetection/LaunchDetector.h | 9 ++++++--- src/lib/launchdetection/LaunchMethod.h | 7 ++++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- 6 files changed, 38 insertions(+), 27 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 1039b4a09..12f80c4a3 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -41,12 +41,16 @@ #include "CatapultLaunchMethod.h" #include -CatapultLaunchMethod::CatapultLaunchMethod() : +namespace launchdetection +{ + +CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : + SuperBlock(parent, "CAT"), last_timestamp(hrt_absolute_time()), integrator(0.0f), launchDetected(false), - threshold_accel(NULL, "LAUN_CAT_A", false), - threshold_time(NULL, "LAUN_CAT_T", false) + threshold_accel(this, "A"), + threshold_time(this, "T") { } @@ -83,14 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected() return launchDetected; } -void CatapultLaunchMethod::updateParams() -{ - threshold_accel.update(); - threshold_time.update(); -} void CatapultLaunchMethod::reset() { integrator = 0.0f; launchDetected = false; } + +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index b8476b4c8..55c46ff3f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -44,17 +44,20 @@ #include "LaunchMethod.h" #include +#include #include -class CatapultLaunchMethod : public LaunchMethod +namespace launchdetection +{ + +class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock { public: - CatapultLaunchMethod(); + CatapultLaunchMethod(SuperBlock *parent); ~CatapultLaunchMethod(); void update(float accel_x); bool getLaunchDetected(); - void updateParams(); void reset(); private: @@ -68,3 +71,5 @@ private: }; #endif /* CATAPULTLAUNCHMETHOD_H_ */ + +} diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 4109a90ba..bf539701b 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -42,12 +42,16 @@ #include "CatapultLaunchMethod.h" #include +namespace launchdetection +{ + LaunchDetector::LaunchDetector() : - launchdetection_on(NULL, "LAUN_ALL_ON", false), - throttlePreTakeoff(NULL, "LAUN_THR_PRE", false) + SuperBlock(NULL, "LAUN"), + launchdetection_on(this, "ALL_ON"), + throttlePreTakeoff(this, "THR_PRE") { /* init all detectors */ - launchMethods[0] = new CatapultLaunchMethod(); + launchMethods[0] = new CatapultLaunchMethod(this); /* update all parameters of all detectors */ @@ -87,12 +91,4 @@ bool LaunchDetector::getLaunchDetected() return false; } -void LaunchDetector::updateParams() { - - launchdetection_on.update(); - throttlePreTakeoff.update(); - - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - launchMethods[i]->updateParams(); - } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 05708c526..8066ebab3 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -45,10 +45,13 @@ #include #include "LaunchMethod.h" - +#include #include -class __EXPORT LaunchDetector +namespace launchdetection +{ + +class __EXPORT LaunchDetector : public control::SuperBlock { public: LaunchDetector(); @@ -57,7 +60,6 @@ public: void update(float accel_x); bool getLaunchDetected(); - void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -72,5 +74,6 @@ private: }; +} #endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index 0cfbab3e0..01fa7050e 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -41,15 +41,20 @@ #ifndef LAUNCHMETHOD_H_ #define LAUNCHMETHOD_H_ +namespace launchdetection +{ + class LaunchMethod { public: virtual void update(float accel_x) = 0; virtual bool getLaunchDetected() = 0; - virtual void updateParams() = 0; virtual void reset() = 0; + protected: private: }; +} + #endif /* LAUNCHMETHOD_H_ */ 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 774758ef4..8ba8f2ccb 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 @@ -186,7 +186,7 @@ private: float target_bearing; /* Launch detection */ - LaunchDetector launchDetector; + launchdetection::LaunchDetector launchDetector; /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s @@ -989,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* no takeoff detection --> fly */ launch_detected = true; + warnx("launchdetection off"); } } -- cgit v1.2.3 From be140eab5998fbec8c485e58e6f61a5ef950053b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 8 Mar 2014 19:15:45 +0100 Subject: geofence: make better use of Block class for updating parameters --- src/modules/navigator/geofence.cpp | 11 ++++------- src/modules/navigator/geofence.h | 6 +++--- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 9bbaf167a..f452a85f7 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -55,11 +55,13 @@ #endif static const int ERROR = -1; -Geofence::Geofence() : _fence_pub(-1), +Geofence::Geofence() : + SuperBlock(NULL, "GF"), + _fence_pub(-1), _altitude_min(0), _altitude_max(0), _verticesCount(0), - param_geofence_on(NULL, "GF_ON", false) + param_geofence_on(this, "ON") { /* Load initial params */ updateParams(); @@ -292,8 +294,3 @@ int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); } - -void Geofence::updateParams() -{ - param_geofence_on.update(); -} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 5b56ebc7a..9628b7271 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -41,11 +41,13 @@ #define GEOFENCE_H_ #include +#include #include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" -class Geofence { +class Geofence : public control::SuperBlock +{ private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -85,8 +87,6 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} - - void updateParams(); }; -- cgit v1.2.3 From 008efff42ea0fec64173941bd8a746ec0e7ecfa4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 20:00:06 +0100 Subject: Added support for driver rangefinder --- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/sf0x/module.mk | 40 ++ src/drivers/sf0x/sf0x.cpp | 829 ++++++++++++++++++++++++++++++++++ 3 files changed, 870 insertions(+) create mode 100644 src/drivers/sf0x/module.mk create mode 100644 src/drivers/sf0x/sf0x.cpp diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a9..aac5beeb1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,6 +27,7 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx +MODULES += drivers/sf0x MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk new file mode 100644 index 000000000..dc93a90e7 --- /dev/null +++ b/src/drivers/sf0x/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the Lightware laser range finder driver. +# + +MODULE_COMMAND = sf0x + +SRCS = sf0x.cpp diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp new file mode 100644 index 000000000..40be85b27 --- /dev/null +++ b/src/drivers/sf0x/sf0x.cpp @@ -0,0 +1,829 @@ +/**************************************************************************** + * + * 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 sf0x.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#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 + +/* Configuration Constants */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +#define SF0X_CONVERSION_INTERVAL 84000 +#define SF0X_TAKE_RANGE_REG 'D' +#define SF02F_MIN_DISTANCE 0.0f +#define SF02F_MAX_DISTANCE 40.0f + +class SF0X : public device::CDev +{ +public: + SF0X(const char* port="/dev/ttyS1"); + virtual ~SF0X(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _fd; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE + * and SF0X_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); + +SF0X::SF0X(const char *port) : + CDev("SF0X", RANGE_FINDER_DEVICE_PATH), + _min_distance(SF02F_MIN_DISTANCE), + _max_distance(SF02F_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), + _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) +{ + /* open fd */ + _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +SF0X::~SF0X() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } +} + +int +SF0X::init() +{ + int ret = ERROR; + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +SF0X::probe() +{ + return measure(); +} + +void +SF0X::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SF0X::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SF0X::get_minimum_distance() +{ + return _min_distance; +} + +float +SF0X::get_maximum_distance() +{ + return _max_distance; +} + +int +SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(SF0X_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +SF0X::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(SF0X_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +SF0X::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + char cmd = SF0X_TAKE_RANGE_REG; + ret = ::write(_fd, &cmd, 1); + + if (OK != sizeof(cmd)) { + perf_count(_comms_errors); + log("serial transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SF0X::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + float val; + + perf_begin(_sample_perf); + + char buf[16]; + ret = ::read(_fd, buf, sizeof(buf)); + printf("ret: %d, val: %s", ret, buf); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + float si_units = 0.0f; + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +SF0X::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +SF0X::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SF0X::cycle_trampoline(void *arg) +{ + SF0X *dev = (SF0X *)arg; + + dev->cycle(); +} + +void +SF0X::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(SF0X_CONVERSION_INTERVAL)); +} + +void +SF0X::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace sf0x +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SF0X *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new SF0X(); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +sf0x_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + sf0x::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + sf0x::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + sf0x::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + sf0x::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + sf0x::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} -- cgit v1.2.3 From 7cc33b9ca6c0facd6e2f38f0bc515e9769e13073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 21:06:29 +0100 Subject: Complete output parsing, pending testing --- src/drivers/sf0x/sf0x.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 40be85b27..d3043a26b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -33,7 +33,8 @@ /** * @file sf0x.cpp - * @author Lorenz Meier + * @author Lorenz Meier + * @author Greg Hulands * * Driver for the Lightware SF0x laser rangefinder series */ @@ -459,25 +460,26 @@ SF0X::measure() int SF0X::collect() { - int ret = -EIO; - - /* read from the sensor */ - float val; + int ret; perf_begin(_sample_perf); char buf[16]; + /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - printf("ret: %d, val: %s", ret, buf); - if (ret < 0) { + if (ret < 1) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - float si_units = 0.0f; + printf("ret: %d, val (str): %s\n", ret, buf); + char* end; + float si_units = strtod(buf,&end); + printf("val (float): %8.4f\n", si_units); + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -538,7 +540,7 @@ SF0X::stop() void SF0X::cycle_trampoline(void *arg) { - SF0X *dev = (SF0X *)arg; + SF0X *dev = static_cast(arg); dev->cycle(); } @@ -598,7 +600,7 @@ SF0X::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %d ticks\n", _measure_ticks); _reports->print_info("report queue"); } @@ -694,7 +696,6 @@ test() { struct range_finder_report report; ssize_t sz; - int ret; int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); @@ -725,7 +726,7 @@ test() /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + int ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); -- cgit v1.2.3 From 171af566f716a99b078eaa4ef0f90405091e8d19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 21:45:22 +0100 Subject: Allow to set the UART via start argument, cleanups --- src/drivers/sf0x/sf0x.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d3043a26b..f25856689 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -87,11 +87,12 @@ static const int ERROR = -1; #define SF0X_TAKE_RANGE_REG 'D' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f +#define SF0X_DEFAULT_PORT "/dev/ttyS2" class SF0X : public device::CDev { public: - SF0X(const char* port="/dev/ttyS1"); + SF0X(const char* port=SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -628,7 +629,7 @@ void info(); * Start the driver. */ void -start() +start(const char* port) { int fd; @@ -637,7 +638,7 @@ start() } /* create the driver */ - g_dev = new SF0X(); + g_dev = new SF0X(port); if (g_dev == nullptr) { goto fail; @@ -795,7 +796,11 @@ sf0x_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(argv[1], "start")) { - sf0x::start(); + if (argc > 2) { + sf0x::start(argv[2]); + } else { + sf0x::start(SF0X_DEFAULT_PORT); + } } /* -- cgit v1.2.3 From d69d895f0240a0dda5fda2f8f046179b069439a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 00:46:13 +0100 Subject: Add missing CDev init step --- src/drivers/sf0x/sf0x.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index f25856689..5dd1f59de 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -209,6 +209,10 @@ SF0X::init() { int ret = ERROR; + /* do regular cdev init */ + if (CDev::init() != OK) + goto out; + /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -447,7 +451,7 @@ SF0X::measure() char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); - if (OK != sizeof(cmd)) { + if (ret != sizeof(cmd)) { perf_count(_comms_errors); log("serial transfer returned %d", ret); return ret; -- cgit v1.2.3 From d2905b8d8eba3a3b1e4258343e7a384071c55073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 11:18:02 +0100 Subject: SF02/F operational, but not cleaned up / optimized - good enough for first logging --- src/drivers/sf0x/sf0x.cpp | 137 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 106 insertions(+), 31 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5dd1f59de..7b4b49813 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -83,8 +84,8 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 84000 -#define SF0X_TAKE_RANGE_REG 'D' +#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f #define SF0X_DEFAULT_PORT "/dev/ttyS2" @@ -186,8 +187,39 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - // enable debug() calls - _debug_enabled = true; + if (_fd < 0) { + warnx("FAIL: laser fd"); + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B9600; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d OSPD\n", termios_state); + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR baud %d ATTR", termios_state); + } + + // disable debug() calls + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -208,6 +240,7 @@ int SF0X::init() { int ret = ERROR; + unsigned i = 0; /* do regular cdev init */ if (CDev::init() != OK) @@ -217,6 +250,7 @@ SF0X::init() _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { + warnx("mem err"); goto out; } @@ -226,12 +260,33 @@ SF0X::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + warnx("advert err"); } - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + /* attempt to get a measurement 5 times */ + while (ret != OK && i < 5) { + + if (measure()) { + ret = ERROR; + _sensor_ok = false; + } + + usleep(100000); + if (collect()) { + ret = ERROR; + _sensor_ok = false; + } else { + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; + } + + i++; + } + + /* close the fd */ + ::close(_fd); + _fd = -1; out: return ret; } @@ -453,7 +508,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("serial transfer returned %d", ret); + log("write fail %d", ret); return ret; } @@ -473,17 +528,30 @@ SF0X::collect() /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - if (ret < 1) { - log("error reading from sensor: %d", ret); + if (ret < 3) { + log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - printf("ret: %d, val (str): %s\n", ret, buf); + if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + /* incomplete read */ + return -1; + } + char* end; - float si_units = strtod(buf,&end); - printf("val (float): %8.4f\n", si_units); + float si_units; + bool valid; + + if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + si_units = -1.0f; + valid = false; + } else { + si_units = strtod(buf,&end); + valid = true; + } + debug("val (float): %8.4f, raw: %s\n", si_units, buf); struct range_finder_report report; @@ -491,7 +559,7 @@ SF0X::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,21 +587,21 @@ SF0X::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + // /* notify about state change */ + // struct subsystem_info_s info = { + // true, + // true, + // true, + // SUBSYSTEM_TYPE_RANGEFINDER + // }; + // static orb_advert_t pub = -1; + + // if (pub > 0) { + // orb_publish(ORB_ID(subsystem_info), pub, &info); + + // } else { + // pub = orb_advertise(ORB_ID(subsystem_info), &info); + // } } void @@ -553,6 +621,12 @@ SF0X::cycle_trampoline(void *arg) void SF0X::cycle() { + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + } + /* collection phase? */ if (_collect_phase) { @@ -653,9 +727,10 @@ start(const char* port) } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(RANGE_FINDER_DEVICE_PATH, 0); if (fd < 0) { + warnx("device open fail"); goto fail; } -- cgit v1.2.3 From 8267bdf4a5aa857db0f970da750d782f1d8ec369 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 9 Mar 2014 15:50:41 +0100 Subject: fw_att_control: airspeed is now used correctly --- src/modules/fw_att_control/fw_att_control_main.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) 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 17b1028f9..c59a396c2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -230,7 +230,7 @@ private: /** * Check for airspeed updates. */ - bool vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. @@ -452,7 +452,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -bool +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ @@ -462,10 +462,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); // warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); - return true; } - - return false; } void @@ -539,7 +536,7 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -596,7 +593,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - _airspeed_valid = vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -636,8 +633,7 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is smaller than min, the sensor is not giving good readings */ - if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || + if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; -- cgit v1.2.3 From 647142764fa3ffd5d99f4d43950975cc6a19bded Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 9 Mar 2014 22:08:28 +0400 Subject: position_estimator_inav: hotfix, change lower dt limit from 5 ms to 2 ms --- src/modules/position_estimator_inav/inertial_filter.c | 5 +---- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 13328edb4..7cd076948 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3]) void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * dt; - if (ewdt > 1.0f) - ewdt = 1.0f; // prevent over-correcting - ewdt *= e; + float ewdt = e * w * dt; x[i] += ewdt; if (i == 0) { 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 d6d03367b..a14354138 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.005); + dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; /* use GPS if it's valid and reference position initialized */ -- cgit v1.2.3 From 1c488a95da934817999a0a40ccb87e07bf5e0307 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:46:22 +0100 Subject: Added laser range finder logging, needs testing --- src/drivers/sf0x/sf0x.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 7b4b49813..685b96646 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -118,6 +118,9 @@ private: int _measure_ticks; bool _collect_phase; int _fd; + char _linebuf[10]; + unsigned _linebuf_index; + hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -179,6 +182,9 @@ SF0X::SF0X(const char *port) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _fd(-1), + _linebuf_index(0), + _last_read(0), _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -187,6 +193,10 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + ::write(_fd, &stop_auto, 1); + if (_fd < 0) { warnx("FAIL: laser fd"); } @@ -219,7 +229,7 @@ SF0X::SF0X(const char *port) : } // disable debug() calls - _debug_enabled = false; + _debug_enabled = true; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -524,18 +534,26 @@ SF0X::collect() perf_begin(_sample_perf); - char buf[16]; + /* clear buffer if last read was too long ago */ + if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + _linebuf_index = 0; + } + /* read from the sensor (uart buffer) */ - ret = ::read(_fd, buf, sizeof(buf)); + ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 3) { + if (ret < 1) { log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + _linebuf_index += ret; + + _last_read = hrt_absolute_time(); + + if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { /* incomplete read */ return -1; } @@ -544,7 +562,7 @@ SF0X::collect() float si_units; bool valid; - if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { -- cgit v1.2.3 From 3f3abebf688c97f87fd6d6016aac5d234d76893f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:47:31 +0100 Subject: Add range finder to logging --- src/modules/sdlog2/sdlog2.c | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e70..53f07651f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,8 @@ #include #include +#include + #include #include #include @@ -759,6 +761,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; + struct range_finder_report range_finder; } buf; memset(&buf, 0, sizeof(buf)); @@ -785,6 +788,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_sub; + int range_finder_sub; } subs; /* log message buffer: header + body */ @@ -955,6 +959,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- RANGE FINDER --- */ + subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + fds[fdsc_count].fd = subs.range_finder_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. @@ -1370,6 +1380,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(TELE); } + /* --- BOTTOM DISTANCE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder); + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom_rate = 0.0f; + log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } + /* 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 */ -- cgit v1.2.3 From 30a57421f8027cb9400bea71cb3b9c2b4f5094ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:48:07 +0100 Subject: Dropped default gains --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index d691a6f2e..f11aa704e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -10,11 +10,11 @@ sh /etc/init.d/rc.mc_defaults if [ $DO_AUTOCONFIG == yes ] then # TODO tune roll/pitch separately - param set MC_ROLL_P 9.0 + param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 9.0 + param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.004 -- cgit v1.2.3 From ddb94ceba8f8813a8d434d00daf42a232e58cbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Mar 2014 23:39:31 +0400 Subject: attitude_estimator_ekf: hotfix, do mag declination rotation matrix initialization --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 620185fb7..00bd23f83 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -477,6 +477,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); + x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; x_aposteriori_k[2] = z_k[2]; -- cgit v1.2.3 From e0cf0e3f41d0395a902032fc1cf78bc9ca76362c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 10 Mar 2014 22:46:53 +0100 Subject: fw posctrl: change 2 functions that return nothing from int to void --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 8ba8f2ccb..7f13df785 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 @@ -350,12 +350,12 @@ private: /* * Reset takeoff state */ - int reset_takeoff_state(); + void reset_takeoff_state(); /* * Reset landing state */ - int reset_landing_state(); + void reset_landing_state(); }; namespace l1_control @@ -1312,14 +1312,14 @@ FixedwingPositionControl::task_main() _exit(0); } -int FixedwingPositionControl::reset_takeoff_state() +void FixedwingPositionControl::reset_takeoff_state() { launch_detected = false; usePreTakeoffThrust = false; launchDetector.reset(); } -int FixedwingPositionControl::reset_landing_state() +void FixedwingPositionControl::reset_landing_state() { land_noreturn_horizontal = false; land_noreturn_vertical = false; -- cgit v1.2.3 From b3839afbbc904fe0009610dae57a4626b3306fb6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 11:12:39 +0400 Subject: 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 788fe5732..4bc1055d1 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 b52c12796..c606da504 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 ef1a747da..c9fcc4cd8 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)); -- cgit v1.2.3 From 290b07920c94df09415ccc1c44850c57d0750fc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:26 +0100 Subject: Fixed missing reset of poll rate after test exit in ultrasound driver --- src/drivers/mb12xx/mb12xx.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7693df295..5adb8cf69 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) { // 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)); @@ -762,6 +762,11 @@ test() warnx("time: %lld", report.timestamp); } + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + errx(0, "PASS"); } -- cgit v1.2.3 From 7851472ae9f19341a1a8905eed63c2c907f16385 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:53 +0100 Subject: Laser range finder ready to roll, logging tested --- src/drivers/sf0x/sf0x.cpp | 68 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 52 insertions(+), 16 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 685b96646..dace02cf8 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -84,7 +84,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_CONVERSION_INTERVAL 83334 #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f @@ -193,14 +193,16 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - /* tell it to stop auto-triggering */ - char stop_auto = ' '; - ::write(_fd, &stop_auto, 1); - if (_fd < 0) { warnx("FAIL: laser fd"); } + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + (void)::write(_fd, &stop_auto, 1); + usleep(100); + (void)::write(_fd, &stop_auto, 1); + struct termios uart_config; int termios_state; @@ -229,7 +231,7 @@ SF0X::SF0X(const char *port) : } // disable 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)); @@ -535,41 +537,58 @@ SF0X::collect() perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ - if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } /* read from the sensor (uart buffer) */ - ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 1) { - log("read err: %d", ret); + if (ret < 0) { + _linebuf[sizeof(_linebuf) - 1] = '\0'; + debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - return ret; + + /* only throw an error if we time out */ + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + return ret; + } else { + return -EAGAIN; + } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { + _linebuf_index = 0; + } _last_read = hrt_absolute_time(); if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read */ - return -1; + /* incomplete read, reschedule ourselves */ + return -EAGAIN; } char* end; float si_units; bool valid; + /* enforce line ending */ + _linebuf[sizeof(_linebuf) - 1] = '\0'; + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(buf,&end); + si_units = strtod(_linebuf,&end); valid = true; } - debug("val (float): %8.4f, raw: %s\n", si_units, buf); + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + + /* done with this chunk, resetting */ + _linebuf_index = 0; struct range_finder_report report; @@ -649,7 +668,19 @@ SF0X::cycle() if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(1100)); + return; + } + + if (OK != collect_ret) { log("collection error"); /* restart the measurement state machine */ start(); @@ -842,6 +873,11 @@ test() warnx("time: %lld", report.timestamp); } + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + errx(0, "PASS"); } -- cgit v1.2.3 From b7662537df656f8546acecc9cca6ab2ac40714bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:54:48 +0100 Subject: Fix code style of laser rangefinder --- src/drivers/sf0x/sf0x.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index dace02cf8..70cd1ab1e 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; class SF0X : public device::CDev { public: - SF0X(const char* port=SF0X_DEFAULT_PORT); + SF0X(const char *port = SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -255,8 +255,9 @@ SF0X::init() unsigned i = 0; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -284,9 +285,11 @@ SF0X::init() } usleep(100000); + if (collect()) { ret = ERROR; _sensor_ok = false; + } else { ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -538,6 +541,7 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } @@ -550,16 +554,18 @@ SF0X::collect() debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - + /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; + } else { return -EAGAIN; } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { _linebuf_index = 0; } @@ -571,7 +577,7 @@ SF0X::collect() return -EAGAIN; } - char* end; + char *end; float si_units; bool valid; @@ -581,10 +587,12 @@ SF0X::collect() if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; + } else { - si_units = strtod(_linebuf,&end); + si_units = strtod(_linebuf, &end); valid = true; } + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); /* done with this chunk, resetting */ @@ -756,7 +764,7 @@ void info(); * Start the driver. */ void -start(const char* port) +start(const char *port) { int fd; @@ -931,6 +939,7 @@ sf0x_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (argc > 2) { sf0x::start(argv[2]); + } else { sf0x::start(SF0X_DEFAULT_PORT); } -- cgit v1.2.3 From 2db140bcabaafcccc1ed2ce070d7a24b4133eb57 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 10:59:11 +0100 Subject: fw_att_control: delete the unused flag --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 c59a396c2..ec91f4d8c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -136,7 +136,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ struct { float tconst; @@ -293,8 +292,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false), - _airspeed_valid(false) + _setpoint_valid(false) { /* safely initialize structs */ _att = {0}; @@ -753,10 +751,6 @@ FixedwingAttitudeControl::task_main() warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); } - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); - /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available -- cgit v1.2.3 From 2cd29b7c8ce7fc26af6f1a3957e243a18bc09d5b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:07:48 +0100 Subject: fw_att_control: proper struct initialization --- src/modules/fw_att_control/fw_att_control_main.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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 ec91f4d8c..fa0f5962f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -295,15 +295,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _setpoint_valid(false) { /* safely initialize structs */ - _att = {0}; - _accel = {0}; - _att_sp = {0}; - _manual = {0}; - _airspeed = {0}; - _vcontrol_mode = {0}; - _actuators = {0}; - _actuators_airframe = {0}; - _global_pos = {0}; + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); -- cgit v1.2.3 From 2f06f15240a9d03a282212538283e53e9bcc4b9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:09:49 +0100 Subject: fw_att_control: whitespace only --- src/modules/fw_att_control/fw_att_control_main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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 fa0f5962f..deafd7f86 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -108,14 +108,14 @@ 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 _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ @@ -123,15 +123,15 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ -- cgit v1.2.3 From 0ad38a73231877ec142946cb4c380c74a1633e63 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 15:34:27 +0400 Subject: 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 4bc1055d1..2dfc67fd4 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 c606da504..ffc69bc81 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. -- cgit v1.2.3 From 35b8d00e17cc8161e9d2687af3f9313d010c11ea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 16:20:37 +0400 Subject: 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 2dfc67fd4..4c79c67b4 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 ffc69bc81..ccff6614f 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[]); }; -- cgit v1.2.3 From 09f18408fc5835622a955c80304ce6599ef98090 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 17:21:03 +0400 Subject: 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 ccff6614f..cc4c896b9 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; -- cgit v1.2.3 From 9e41f6af18d3d84413501ce37737d574fd20816d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 19:28:48 +0400 Subject: 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 4c79c67b4..5bdf6c262 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 c9fcc4cd8..fa63e06c5 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 -- cgit v1.2.3 From 1fd7b89d3c7c5f9b5c13198c41a015d6c5899260 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:03:49 +0400 Subject: mavlink: use "normal" mode for HIL operation, only add HIL_CONTROLS stream when HIL enabled --- src/modules/mavlink/mavlink_main.cpp | 117 +++++++---------------------------- src/modules/mavlink/mavlink_main.h | 10 ++- 2 files changed, 25 insertions(+), 102 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bdf6c262..e1c53a599 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,20 +1615,12 @@ 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"); + case MAVLINK_MODE_NORMAL: + warnx("mode: NORMAL"); break; - case MODE_HIL: - warnx("mode: HIL"); + case MAVLINK_MODE_CUSTOM: + warnx("mode: CUSTOM"); break; default: @@ -1670,21 +1628,7 @@ Mavlink::task_main(int argc, char *argv[]) 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 cc4c896b9..3a8625c6b 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 */ -- cgit v1.2.3 From 4cee3614c7bc2e960ac52e59014bc4d08b8da11e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:04:23 +0400 Subject: 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 0de2d4295..558be4275 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 -- cgit v1.2.3 From 5f847ff8a3df1ba303129a7a4938305a6901755c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 21:14:54 +0100 Subject: 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 eba18ddb1..8cb8f4dc7 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 -- cgit v1.2.3 From a37e539647df2b3cfd335b33e4d186c4d39316b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 22:26:12 +0100 Subject: 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 5bdf6c262..667580452 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(); -- cgit v1.2.3 From a977ff2b6295bd4bac614a854ecc35e1b9b2d75c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 00:20:27 +0100 Subject: fw att control: add timestamp dependency in airspeed check --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 deafd7f86..497d1d15a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -632,7 +632,8 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { + !isfinite(_airspeed.indicated_airspeed_m_s) || + (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { airspeed = _parameters.airspeed_trim; } else { -- cgit v1.2.3 From f66b0ad8ac926a79608cf872c8e49ea7ace92288 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:04:30 +0100 Subject: 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 667580452..ed3b265f9 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 cc4c896b9..2a7bd57cf 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 */ -- cgit v1.2.3 From 023c7069c4a340aa1b7c9f05963a066cee368b2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:36:12 +0100 Subject: 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 ed3b265f9..9a6e1130e 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); -- cgit v1.2.3 From 76af0970f5220d3a842c4daa353a45ef51df1082 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:46:02 +0100 Subject: 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 667580452..1044984bf 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 fa63e06c5..c222a3ddf 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); } } } -- cgit v1.2.3 From 2d2ecbad00c4f21d0c3ded2faa4f9bdb7adefddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:56:26 +0100 Subject: 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 1044984bf..d722bec46 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; -- cgit v1.2.3 From d9d0e60bd574b135d98bdaa48c4b32056f815217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 10:02:39 +0100 Subject: fw att ctrl: airspeed check: remove unnecessary cast --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 497d1d15a..bb674d6c9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -633,7 +633,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s) || - (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { -- cgit v1.2.3 From 7604518db479e7cc9618e19b224c5d5c94013142 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 10:29:39 +0100 Subject: 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 7e1af8824..4420e04fd 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; -- cgit v1.2.3 From 45a18587fc5e96697da586b9bf51ee65c10e3753 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 17:53:25 +0100 Subject: 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 4420e04fd..b699b86a5 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 2a7bd57cf..94b2c9dbc 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. * -- cgit v1.2.3 From 6f550775410b0045bce5dafd0d4fd3f03988f8bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Mar 2014 00:24:28 +0400 Subject: gpio_led: bugs fixed --- src/modules/gpio_led/gpio_led.c | 41 +++++++++++++++++------------------------ 1 file changed, 17 insertions(+), 24 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index f5f3dea76..93a29a851 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -91,8 +91,7 @@ int gpio_led_main(int argc, char *argv[]) #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 errx(1, "usage: gpio_led {start|stop} [-p ]\n" - "\t-p\tUse pin:\n" - "\t\tn\tAUX OUT pin number (default: 1)\n" + "\t-p \tUse specified AUX OUT pin number (default: 1)" ); #endif @@ -108,6 +107,14 @@ int gpio_led_main(int argc, char *argv[]) /* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */ int pin = 1; + /* pin name to display */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + char *pin_name = "PX4FMU GPIO_EXT1"; +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + char pin_name[] = "AUX OUT 1"; +#endif + if (argc > 2) { if (!strcmp(argv[2], "-p")) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -115,26 +122,32 @@ int gpio_led_main(int argc, char *argv[]) if (!strcmp(argv[3], "1")) { use_io = false; pin = GPIO_EXT_1; + pin_name = "PX4FMU GPIO_EXT1"; } else if (!strcmp(argv[3], "2")) { use_io = false; pin = GPIO_EXT_2; + pin_name = "PX4FMU GPIO_EXT2"; } else if (!strcmp(argv[3], "a1")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_ACC1; + pin_name = "PX4IO ACC1"; } else if (!strcmp(argv[3], "a2")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_ACC2; + pin_name = "PX4IO ACC2"; } else if (!strcmp(argv[3], "r1")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_POWER1; + pin_name = "PX4IO RELAY1"; } else if (!strcmp(argv[3], "r2")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_POWER2; + pin_name = "PX4IO RELAY2"; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -146,7 +159,8 @@ int gpio_led_main(int argc, char *argv[]) if (n >= 1 && n <= 6) { use_io = false; - pin = n; + pin = 1 << (n - 1); + snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n); } else { errx(1, "unsupported pin: %s", argv[3]); @@ -166,27 +180,6 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; - char pin_name[24]; - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - if (use_io) { - if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { - sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); - - } else { - sprintf(pin_name, "PX4IO RELAY%i", pin); - } - - } else { - sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); - } - -#endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - sprintf(pin_name, "AUX OUT %i", pin); -#endif - warnx("start, using pin: %s", pin_name); } -- cgit v1.2.3 From bc36b540a5608f17d7c14bc87fb2bb7ca76459b2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Mar 2014 00:25:00 +0400 Subject: gpio_led: code style fixed --- src/modules/gpio_led/gpio_led.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 93a29a851..6dfd22fdf 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -253,8 +253,9 @@ void gpio_led_cycle(FAR void *arg) bool updated; orb_check(priv->vehicle_status_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + } /* select pattern for current status */ int pattern = 0; @@ -294,8 +295,9 @@ void gpio_led_cycle(FAR void *arg) priv->counter++; - if (priv->counter > 5) + if (priv->counter > 5) { priv->counter = 0; + } /* repeat cycle at 5 Hz */ if (gpio_led_started) { -- cgit v1.2.3 From b6ea38b91a3d161ce01ab3683b0097f45c4e4de3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 13:32:24 +0100 Subject: sensors app: add timestamp in airspeed message --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417..6e2df89ed 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1030,6 +1030,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_counter++; + _airspeed.timestamp = hrt_absolute_time(); _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, raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); -- cgit v1.2.3 From 5745233f803d57f3ad3cb0e95bbc99ed2392728c Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 13 Mar 2014 14:36:02 +0100 Subject: Remove non-existent parameter from 4008_ardrone startup script. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 0f98f7b6c..045a4ce00 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -23,7 +23,6 @@ then param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.0 param set MC_YAW_P 1.0 - param set MC_YAW_D 0.1 param set MC_YAWRATE_P 0.15 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 -- cgit v1.2.3 From 7a97eb83f629ec718a6e31ccb5d184786d0a5377 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 15:25:43 +0100 Subject: ardrone startup: set battery V scale parameter in autoconfig to the value given in sensors_params.c --- ROMFS/px4fmu_common/init.d/4008_ardrone | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 045a4ce00..3a17b34ab 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -27,6 +27,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.15 + param set BAT_V_SCALING 0.00838095238 fi set OUTPUT_MODE ardrone -- cgit v1.2.3 From bfe8d735604511da44c63e74efa1cba5eb694064 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 15:41:53 +0100 Subject: rcS: introduce flag that allows disabling startup of the default apps in order to start a different set of apps in a autostart script --- ROMFS/px4fmu_common/init.d/rcS | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 57299d772..55e2a886e 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 MAV_TYPE none + set LOAD_DEFAULT_APPS yes # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -465,7 +466,10 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - sh /etc/init.d/rc.fw_apps + if [ LOAD_DEFAULT_APPS == yes ] + then + sh /etc/init.d/rc.fw_apps + fi fi # @@ -521,7 +525,10 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - sh /etc/init.d/rc.mc_apps + if [ LOAD_DEFAULT_APPS == yes ] + then + sh /etc/init.d/rc.mc_apps + fi fi # -- cgit v1.2.3 From 11f12b4dfed0a5209e34a3d7cc1e473c0649aca6 Mon Sep 17 00:00:00 2001 From: dominiho Date: Thu, 13 Mar 2014 15:43:52 +0100 Subject: added i2c px4flow driver --- src/drivers/drv_px4flow.h | 84 +++++ src/drivers/px4flow/module.mk | 40 ++ src/drivers/px4flow/px4flow.cpp | 806 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 930 insertions(+) create mode 100644 src/drivers/drv_px4flow.h create mode 100644 src/drivers/px4flow/module.mk create mode 100644 src/drivers/px4flow/px4flow.cpp diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h new file mode 100644 index 000000000..bef02d54e --- /dev/null +++ b/src/drivers/drv_px4flow.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 Rangefinder driver interface. + */ + +#ifndef _DRV_PX4FLOW_H +#define _DRV_PX4FLOW_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define PX4FLOW_DEVICE_PATH "/dev/px4flow" + +/** + * Optical flow in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct px4flow_report { + + uint64_t timestamp; /**< in microseconds since system start */ + + int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ + int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ + float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ + float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + +}; + +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(optical_flow); + +/* + * ioctl() definitions + * + * px4flow drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _PX4FLOWIOCBASE (0x7700) +#define __PX4FLOWIOC(_n) (_IOC(_PX4FLOWIOCBASE, _n)) + + +#endif /* _DRV_PX4FLOW_H */ diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk new file mode 100644 index 000000000..d3062e457 --- /dev/null +++ b/src/drivers/px4flow/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the PX4FLOW driver. +# + +MODULE_COMMAND = px4flow + +SRCS = px4flow.cpp diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp new file mode 100644 index 000000000..50089282a --- /dev/null +++ b/src/drivers/px4flow/px4flow.cpp @@ -0,0 +1,806 @@ +/**************************************************************************** + * + * 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 px4flow.cpp + * @author Dominik Honegger + * + * Driver for the PX4FLOW module connected via I2C. + */ + +#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 + +/* Configuration Constants */ +#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION +#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A +//range 0x42 - 0x49 + +/* PX4FLOW Registers addresses */ +#define PX4FLOW_REG 0x00 /* Measure Register */ + +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +//struct i2c_frame +//{ +// uint16_t frame_count; +// int16_t pixel_flow_x_sum; +// int16_t pixel_flow_y_sum; +// int16_t flow_comp_m_x; +// int16_t flow_comp_m_y; +// int16_t qual; +// int16_t gyro_x_rate; +// int16_t gyro_y_rate; +// int16_t gyro_z_rate; +// uint8_t gyro_range; +// uint8_t sonar_timestamp; +// int16_t ground_distance; +//}; +// +//struct i2c_frame f; + +class PX4FLOW : public device::I2C +{ +public: + PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); + virtual ~PX4FLOW(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _px4flow_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); + +PX4FLOW::PX4FLOW(int bus, int address) : + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _px4flow_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), + _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +PX4FLOW::~PX4FLOW() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; +} + +int +PX4FLOW::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(px4flow_report)); + + if (_reports == nullptr) + goto out; + + /* get a publish handle on the px4flow topic */ + struct px4flow_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); + + if (_px4flow_topic < 0) + debug("failed to create px4flow object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +PX4FLOW::probe() +{ + return measure(); +} + +int +PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct px4flow_report); + struct px4flow_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(PX4FLOW_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +PX4FLOW::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = PX4FLOW_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + printf("i2c::transfer flow returned %d"); + return ret; + } + ret = OK; + + return ret; +} + +int +PX4FLOW::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 22); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + +// f.frame_count = val[1] << 8 | val[0]; +// f.pixel_flow_x_sum= val[3] << 8 | val[2]; +// f.pixel_flow_y_sum= val[5] << 8 | val[4]; +// f.flow_comp_m_x= val[7] << 8 | val[6]; +// f.flow_comp_m_y= val[9] << 8 | val[8]; +// f.qual= val[11] << 8 | val[10]; +// f.gyro_x_rate= val[13] << 8 | val[12]; +// f.gyro_y_rate= val[15] << 8 | val[14]; +// f.gyro_z_rate= val[17] << 8 | val[16]; +// f.gyro_range= val[18]; +// f.sonar_timestamp= val[19]; +// f.ground_distance= val[21] << 8 | val[20]; + + int16_t flowcx = val[7] << 8 | val[6]; + int16_t flowcy = val[9] << 8 | val[8]; + int16_t gdist = val[21] << 8 | val[20]; + + struct px4flow_report report; + report.flow_comp_x_m = float(flowcx)/1000.0f; + report.flow_comp_y_m = float(flowcy)/1000.0f; + report.flow_raw_x= val[3] << 8 | val[2]; + report.flow_raw_y= val[5] << 8 | val[4]; + report.ground_distance_m =float(gdist)/1000.0f; + report.quality= val[10]; + report.sensor_id = 0; + report.timestamp = hrt_absolute_time(); + + + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + + /* post a report to the ring */ + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +PX4FLOW::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_OPTICALFLOW}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +PX4FLOW::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +PX4FLOW::cycle_trampoline(void *arg) +{ + PX4FLOW *dev = (PX4FLOW *)arg; + + dev->cycle(); +} + +void +PX4FLOW::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&PX4FLOW::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&PX4FLOW::cycle_trampoline, + this, + USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); +} + +void +PX4FLOW::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace px4flow +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +PX4FLOW *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new PX4FLOW(PX4FLOW_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct px4flow_report report; + ssize_t sz; + int ret; + + int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + // err(1, "immediate read failed"); + + warnx("single read"); + warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); + warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("time: %lld", report.timestamp); + + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); + warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("time: %lld", report.timestamp); + + + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +px4flow_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + px4flow::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + px4flow::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + px4flow::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + px4flow::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + px4flow::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} -- cgit v1.2.3 From 7d48941c02fdb84df2f11731735caaa3b8a2e698 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 18:47:31 +0100 Subject: set correct MAV_TYPE in ardrone startup script --- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 3a17b34ab..14786f210 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -33,3 +33,5 @@ fi set OUTPUT_MODE ardrone set USE_IO no set MIXER skip +# set MAV_TYPE because no specific mixer is set +set MAV_TYPE 2 -- cgit v1.2.3 From 6b79f533388ab78ddbbb8cc316b38c6e7403e46a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 14 Mar 2014 14:40:54 +0100 Subject: 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 d722bec46..f1a1f9568 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) { -- cgit v1.2.3 From f4c91b19e47f8d8e0ee890dc605ee675a3592bad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 13:46:09 +0100 Subject: set phantom autoconfig parameters as provided by Andreas Antener --- ROMFS/px4fmu_common/init.d/3031_phantom | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a3004d1e1..6cbd23643 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,9 +2,38 @@ # # Phantom FPV Flying Wing # -# Simon Wilks +# Simon Wilks , Thomas Gubler # sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 13 + param set FW_AIRSPD_TRIM 18 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.75 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.2 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.03 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.5 + param set FW_RR_I 0.02 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.08 + param set FW_R_LIM 70 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + set MIXER FMU_Q -- cgit v1.2.3 From 75c168f13fea4c413dc7168a8d5696380c3e9ed5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 15:28:37 +0100 Subject: add missing $ in rcS script --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 55e2a886e..7c3524fef 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -466,7 +466,7 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -525,7 +525,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.mc_apps fi -- cgit v1.2.3 From 717e1bd374e7710ce579e91c45852bbba906eba8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:47:26 +0100 Subject: 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 00bd23f83..11ae2a268 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 e79726613..7150218ff 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 a14354138..368fa6ee2 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 2514bafee..ee3ec7216 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 b176d7417..76dc262f1 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 ad164555e..92812efd4 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 */ }; /** -- cgit v1.2.3 From 3874bca2084bb88dcd739b309bd4a7929db3b417 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:48:33 +0100 Subject: mavlink: Only send messages when we have updates for them. --- src/modules/mavlink/mavlink_messages.cpp | 472 ++++++++++++----------- src/modules/mavlink/mavlink_orb_subscription.cpp | 21 +- src/modules/mavlink/mavlink_orb_subscription.h | 9 + src/modules/mavlink/mavlink_receiver.cpp | 9 +- 4 files changed, 281 insertions(+), 230 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d83afe82..7d388f88d 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); - - 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); + 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); + } } }; @@ -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_counter != sensor->gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = sensor->gyro_counter; - } + 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_counter != sensor->magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = sensor->magnetometer_counter; - } + 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_counter != sensor->baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = sensor->baro_counter; - } + 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); + 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); - - 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); + 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); + } } }; @@ -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); - - 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); + 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); + + if (updated) { + + 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); - - 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); + 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); + } } }; @@ -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); - - 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]); + 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]); + } } }; @@ -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; + + default: + n = 8; + break; + } + + /* scale / assign outputs depending on system type */ + float out[8]; - /* 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; - 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); - - 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); + if (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); + } } } }; @@ -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 6279e5366..996318468 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 eacc27034..42d47e96e 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 c222a3ddf..2ce86396c 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) { -- cgit v1.2.3 From df5d702baefae7f51161d170c911cb398f418538 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 17:03:39 +0400 Subject: mavlink: MavlinkOrbSubscription.update() result fixed --- src/modules/mavlink/mavlink_orb_subscription.cpp | 11 +++++++---- src/modules/mavlink/mavlink_orb_subscription.h | 13 +++++++------ 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 996318468..e8f9bb75b 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) { + if (_last_check == t) { + /* already checked right now, return result of the check */ + return _updated; + + } else { _last_check = t; - bool updated; - orb_check(_fd, &updated); + orb_check(_fd, &_updated); - if (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 42d47e96e..8529721c0 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 */ }; -- cgit v1.2.3 From 762e8f52893afee6e8ffb6a728e5bc88518829c3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 17:28:34 +0400 Subject: 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 e8f9bb75b..4de722832 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 8529721c0..5c6543e81 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(); -- cgit v1.2.3 From 15c079921bf2f8256e0fcdd5ab9f31157bc09f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:12:03 +0100 Subject: 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 a736cbdf6..785c8beeb 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(); -- cgit v1.2.3 From 85b7670b44d0af1141fae73593f68d8469846d8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:15:31 +0100 Subject: 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 785c8beeb..c72f692d7 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(); -- cgit v1.2.3 From c1ee1abf274749900a39279f2fed57ad890c2865 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 16 Mar 2014 14:47:29 +0100 Subject: introduce TBS caipirinha autostart script --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 42 ++++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ 2 files changed, 47 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha new file mode 100644 index 000000000..7dbda54d3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -0,0 +1,42 @@ +#!nsh +# +# TBS Caipirinha Flying Wing +# +# Thomas Gubler +# + +sh /etc/init.d/rc.fw_defaults + +if [ $DO_AUTOCONFIG == yes ] +then + + # TODO: these are the X5 default parameters, update them to the caipi + + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.3 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.03 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.3 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.03 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 7aaf7133e..4ae6a2794 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -97,6 +97,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3100 +then + sh /etc/init.d/3100_tbs_caipirinha +fi + # # Quad X # -- cgit v1.2.3 From 8818425e584c9670dfa9a823b3dfd95147c087d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 18:25:09 +0400 Subject: 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 31955d3e5..bf49403d0 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); -- cgit v1.2.3 From bb7962936e15c594821ae10e4510febfec0d6b9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:37:16 +0100 Subject: 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 b157b3f18..6e2ebb1f7 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; } -- cgit v1.2.3 From 772fce9f82749bde8c14199fcc73cfa2235e760e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:38:55 +0100 Subject: 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 31955d3e5..131d6a550 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); -- cgit v1.2.3 From 8383603f7691e3bc66534b58654c4d13547632a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:41:03 +0100 Subject: 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 131d6a550..5537c4e80 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); -- cgit v1.2.3 From 4466dbf0b37f0a80790c346affee1e4ac86bef22 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 16 Mar 2014 18:47:21 +0100 Subject: add SYS_USE_IO param which allows using standard startup scripts for FMU only mode --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- src/modules/systemlib/system_params.c | 11 +++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7c3524fef..65a66a085 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,7 +108,6 @@ then set HIL no set VEHICLE_TYPE none set MIXER none - set USE_IO yes set OUTPUT_MODE none set PWM_OUTPUTS none set PWM_RATE none @@ -129,6 +128,16 @@ then else set DO_AUTOCONFIG no fi + + # + # Set USE_IO flag + # + if param compare SYS_USE_IO 1 + then + set USE_IO yes + else + set USE_IO no + fi # # Set parameters and env variables for selected AUTOSTART diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index cb35a2541..ec2bc3a9a 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); * @group System */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); + +/** + * Set usage of IO board + * + * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + * + * @min 0 + * @max 1 + * @group System + */ +PARAM_DEFINE_INT32(SYS_USE_IO, 1); -- cgit v1.2.3 From 6db2191a71ed0ea38e1fd18886371f099c14d593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:50:20 +0100 Subject: commander: Skip devices we do not want to touch in HIL --- src/modules/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 1e892ed77..9178bcc5a 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"); -- cgit v1.2.3 From 2fe9f65c45ba3aaad549b30e3b34568d07ab2b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:16:18 +0100 Subject: commander: Fixed compile error and some stupidity in usage of path names --- src/modules/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 9178bcc5a..e6f245cf9 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); -- cgit v1.2.3 From 2db7d30400d0240baf4532eeef6cb13403417654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:43:09 +0100 Subject: Do not work on USB UARTs --- src/modules/mavlink/mavlink_main.cpp | 33 ++++++++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 11882f0f4..9c2e0178a 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); - - /* setup output flow control */ - if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + 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"); + } } 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 e6e20eb93..f743c2504 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 */ -- cgit v1.2.3 From 1c0d7988e258510d2a4006b63b266a421ae5152c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 21:38:37 +0100 Subject: Defaults for Wing Wing --- ROMFS/px4fmu_common/init.d/3033_wingwing | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 2af3618d9..22d63fad5 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -7,4 +7,28 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set BAT_N_CELLS 2 + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 11 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 12 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_P_ROLLFF 2 + param set FW_PR_FF 0.6 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.06 + param set FW_RR_FF 0.6 + param set FW_RR_IMA 0.2 + param set FW_RR_P 0.09 +fi + set MIXER FMU_Q -- cgit v1.2.3 From ffd0d1032041601916e2bd2faa395ca0af03659c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:25:22 +0100 Subject: Fix usage of right time stamps --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- src/modules/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 11ae2a268..10a6cd2c5 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 7150218ff..3653defa6 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]; -- cgit v1.2.3 From 8cb5a12cc702ac238f4dc79bf1f1cee3fdee005f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:23 +0100 Subject: 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 2ce86396c..71ea832fd 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 beaae2058..0a5a1b5c7 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; -- cgit v1.2.3 From dbd467fe1f3779b45090f556fd05b056f068cd34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:49 +0100 Subject: 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 76dc262f1..0ed26b54c 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); -- cgit v1.2.3 From 533e3172dc37aeeb2bc4ccc4ee884239d91079f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:24:31 +0100 Subject: 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 8e990aa6f..82f3ba044 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; -- cgit v1.2.3 From 9cdb416d3a6ceb1c74b03f3f046f5d904bf7fc45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:32:02 +0100 Subject: 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 4f58891ed..e390140de 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; } -- cgit v1.2.3 From 75ad1c4a1350d73315f48a1f4b9bdbee2d20fa9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:35:07 +0100 Subject: 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 71ea832fd..489d2bdcb 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); -- cgit v1.2.3 From 51658ac857390f20d3cc9e2e59a485516c4e0814 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:37:32 +0100 Subject: 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 e390140de..13cbfdfa8 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; } -- cgit v1.2.3 From 7718bbebee2730b3b6e4b036f71bc39a4b8414e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Mar 2014 15:20:00 +0100 Subject: startup scripts: romfs_pruner doesn't know the inch symbols when used in eclipse, therefore replace these --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 2 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 2 +- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index b98ab4774..38e65435b 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,7 +2,7 @@ # # UNTESTED UNTESTED! # -# Generic 10” Hexa coaxial geometry +# Generic 10" Hexa coaxial geometry # # Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 655cb6226..99f902a6d 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo coaxial geometry +# Generic 10" Octo coaxial geometry # # Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 345b0e3e4..06c54a41d 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Quad X geometry +# Generic 10" Quad X geometry # # Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 55b31067d..1fb25e5d8 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Quad + geometry +# Generic 10" Quad + geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 7714a508c..34fc6523f 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Hexa X geometry +# Generic 10" Hexa X geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 60db8c069..235e376a6 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Hexa + geometry +# Generic 10" Hexa + geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 411aee114..769217dc7 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo X geometry +# Generic 10" Octo X geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 81132fd3e..28b074a54 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo + geometry +# Generic 10" Octo + geometry # # Anton Babushkin # -- cgit v1.2.3 From 8fe3475b41b76ecf07aa6cd1d73196c17b4c8ebe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Mar 2014 20:12:12 +0100 Subject: mavlink: add onboard function for camera capture commands --- src/modules/mavlink/mavlink_main.cpp | 13 ++++++++++ src/modules/mavlink/mavlink_main.h | 3 ++- src/modules/mavlink/mavlink_messages.cpp | 44 +++++++++++++++++++++++++++++++- 3 files changed, 58 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9c2e0178a..2457a7cae 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1653,6 +1653,8 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + } else if (strcmp(optarg, "camera") == 0) { + _mode = MAVLINK_MODE_CAMERA; } break; @@ -1696,6 +1698,10 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; + case MAVLINK_MODE_CAMERA: + warnx("mode: CAMERA"); + break; + default: warnx("ERROR: Unknown mode"); break; @@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); break; + case MAVLINK_MODE_CAMERA: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("ATTITUDE", 20.0f); + configure_stream("GLOBAL_POSITION_INT", 20.0f); + configure_stream("CAMERA_CAPTURE", 1.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f743c2504..5a118a0ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -146,7 +146,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_CAMERA }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88d..014b53829 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -230,7 +230,6 @@ protected: mavlink_base_mode, mavlink_custom_mode, mavlink_state); - } }; @@ -1127,6 +1126,48 @@ protected: } }; +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() + { + return "CAMERA_CAPTURE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCameraCapture(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + + } + + void send(const hrt_abstime t) + { + (void)status_sub->update(t); + + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + + /* send camera capture on */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + } else { + /* send camera capture off */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,6 +1192,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamCameraCapture(), nullptr }; -- cgit v1.2.3 From c5a1f4617c6d3e1782dde5d4552122059200db84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 18:30:05 +0100 Subject: Fixed MS4525 error flag handling --- src/drivers/meas_airspeed/meas_airspeed.cpp | 115 ++++++++++++++++++---------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf7..7baa8738f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -104,7 +104,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); protected: @@ -123,7 +123,7 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, +MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path) { @@ -161,23 +161,30 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - uint8_t status = val[0] & 0xC0; + uint8_t status = (val[0] & 0xC0) >> 6; - if (status == 2) { - log("err: stale data"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } else if (status == 3) { - log("err: fault"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; + switch (status) { + case 0: break; + + case 1: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 2: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; @@ -193,19 +200,21 @@ MEASAirspeed::collect() */ const float P_min = -1.0f; const float P_max = 1.0f; - 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; + 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). */ if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + _max_differential_pressure_pa = diff_press_pa; } report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.voltage = 0; @@ -261,8 +270,9 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -303,15 +313,17 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver, try the MS4525DO first */ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { @@ -319,22 +331,26 @@ start(int i2c_bus) g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -379,21 +395,24 @@ test() int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -404,14 +423,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); @@ -419,8 +440,9 @@ test() } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -433,14 +455,17 @@ reset() { int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -451,8 +476,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -491,32 +517,37 @@ meas_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { meas_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { meas_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { meas_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { meas_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { meas_airspeed::info(); + } meas_airspeed_usage(); exit(0); -- cgit v1.2.3 From fc877f9c20aaeb59275c7e1953bad818ba27662c Mon Sep 17 00:00:00 2001 From: dominiho Date: Wed, 19 Mar 2014 16:13:17 +0100 Subject: changed px4flow I2C address to default value --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 50089282a..f214b5964 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -74,7 +74,7 @@ /* Configuration Constants */ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION -#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A +#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -- cgit v1.2.3 From 952fd1ec6cbc615d6bd9213061dcce0f9cf8c5f0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 11:38:41 +0100 Subject: mavlink: camera mode with rate multiplier --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2457a7cae..dca248dc9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,8 +1773,8 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 20.0f); - configure_stream("GLOBAL_POSITION_INT", 20.0f); + configure_stream("ATTITUDE", 20.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 20.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break; -- cgit v1.2.3 From a989dd6eec5ec15e6387769d8f657a29d68fabff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 14:23:23 +0100 Subject: mavlink: correct verbose info, bytes not bits --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dca248dc9..576b3ddec 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1709,7 +1709,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate); + warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); -- cgit v1.2.3 From bf69a7b6473adde22b3220b6e72c0d065c897bc5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 14:26:45 +0100 Subject: mavlink: camera mode rate is now correct --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 576b3ddec..a36ad0bf6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,8 +1773,8 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 20.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 20.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break; -- cgit v1.2.3 From 2c32cdf16bb02a9ae4d47b60cb32553fefb33738 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 15:54:35 -0400 Subject: Fixed block param template. --- src/modules/controllib/block/BlockParam.cpp | 25 ++++++++++++++++++ src/modules/controllib/block/BlockParam.hpp | 39 ++++++++--------------------- 2 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index fd12e365d..8f98da74f 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref printf("error finding param: %s\n", fullname); }; +template +BlockParam::BlockParam(Block *block, const char *name, + bool parent_prefix) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); +} + +template +T BlockParam::get() { return _val; } + +template +void BlockParam::set(T val) { _val = val; } + +template +void BlockParam::update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); +} + +template +BlockParam::~BlockParam() {}; + +template class __EXPORT BlockParam; +template class __EXPORT BlockParam; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 36bc8c24b..735b2b292 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -70,38 +70,21 @@ protected: * Parameters that are tied to blocks for updating and nameing. */ -class __EXPORT BlockParamFloat : public BlockParamBase +template +class BlockParam : public BlockParamBase { public: - BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - float get() { return _val; } - void set(float val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } + BlockParam(Block *block, const char *name, + bool parent_prefix=true); + T get(); + void set(T val); + void update(); + virtual ~BlockParam(); protected: - float _val; + T _val; }; -class __EXPORT BlockParamInt : public BlockParamBase -{ -public: - BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - int get() { return _val; } - void set(int val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } -protected: - int _val; -}; +typedef BlockParam BlockParamFloat; +typedef BlockParam BlockParamInt; } // namespace control -- cgit v1.2.3 From fd6590cfa7e14436fa8af6a0569b6382cd39069a Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 16:14:56 -0400 Subject: Moved UOrbPubliction/Subscription to uORB::Publication/Subscription --- src/drivers/md25/md25.cpp | 4 +- src/drivers/md25/md25.hpp | 4 +- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/drivers/roboclaw/RoboClaw.hpp | 4 +- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 16 +-- src/modules/controllib/block/Block.cpp | 8 +- src/modules/controllib/block/Block.hpp | 16 ++- src/modules/controllib/module.mk | 2 - src/modules/controllib/uorb/UOrbPublication.cpp | 39 ------- src/modules/controllib/uorb/UOrbPublication.hpp | 118 ------------------- src/modules/controllib/uorb/UOrbSubscription.cpp | 51 --------- src/modules/controllib/uorb/UOrbSubscription.hpp | 137 ----------------------- src/modules/controllib/uorb/blocks.hpp | 22 ++-- src/modules/uORB/Publication.cpp | 39 +++++++ src/modules/uORB/Publication.hpp | 115 +++++++++++++++++++ src/modules/uORB/Subscription.cpp | 51 +++++++++ src/modules/uORB/Subscription.hpp | 134 ++++++++++++++++++++++ src/modules/uORB/module.mk | 4 +- 18 files changed, 382 insertions(+), 384 deletions(-) delete mode 100644 src/modules/controllib/uorb/UOrbPublication.cpp delete mode 100644 src/modules/controllib/uorb/UOrbPublication.hpp delete mode 100644 src/modules/controllib/uorb/UOrbSubscription.cpp delete mode 100644 src/modules/controllib/uorb/UOrbSubscription.hpp create mode 100644 src/modules/uORB/Publication.cpp create mode 100644 src/modules/uORB/Publication.hpp create mode 100644 src/modules/uORB/Subscription.cpp create mode 100644 src/modules/uORB/Subscription.hpp diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d43e3aef9..6d5e805ea 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include @@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu float prev_revolution = md25.getRevolutions1(); // debug publication - control::UOrbPublication debug_msg(NULL, + uORB::Publication debug_msg(NULL, ORB_ID(debug_key_value)); // sine wave for motor 1 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 1661f67f9..962c6b881 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription _actuators; + uORB::Subscription _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d65a9be36..dd5e4d3e0 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -53,7 +53,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index e9f35cf95..58994d6fa 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription _actuators; + uORB::Subscription _actuators; // private data float _motor1Position; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8..caf93bc78 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -138,13 +138,13 @@ protected: 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. */ - control::UOrbSubscription _gps; /**< gps sub. */ - control::UOrbSubscription _param_update; /**< parameter update sub. */ + uORB::Subscription _sensors; /**< sensors sub. */ + uORB::Subscription _gps; /**< gps sub. */ + uORB::Subscription _param_update; /**< parameter update sub. */ // publications - control::UOrbPublication _pos; /**< position pub. */ - control::UOrbPublication _localPos; /**< local position pub. */ - control::UOrbPublication _att; /**< attitude pub. */ + uORB::Publication _pos; /**< position pub. */ + uORB::Publication _localPos; /**< local position pub. */ + uORB::Publication _att; /**< attitude pub. */ // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index b964d40a3..0b4cce979 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "../uorb/UOrbSubscription.hpp" -#include "../uorb/UOrbPublication.hpp" +#include "uORB/Subscription.hpp" +#include "uORB/Publication.hpp" namespace control { @@ -100,7 +100,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - UOrbSubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionBase *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -118,7 +118,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - UOrbPublicationBase *pub = getPublications().getHead(); + uORB::PublicationBase *pub = getPublications().getHead(); int count = 0; while (pub != NULL) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 258701f27..6a8e1b5b9 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -44,6 +44,12 @@ #include "List.hpp" +// forward declaration +namespace uORB { + class SubscriptionBase; + class PublicationBase; +} + namespace control { @@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80; // forward declaration class BlockParamBase; -class UOrbSubscriptionBase; -class UOrbPublicationBase; class SuperBlock; /** @@ -79,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } + List & getSubscriptions() { return _subscriptions; } + List & getPublications() { return _publications; } List & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List _subscriptions; - List _publications; + List _subscriptions; + List _publications; List _params; }; diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index d815a8feb..f0139a4b7 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,5 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - uorb/UOrbPublication.cpp \ - uorb/UOrbSubscription.cpp \ uorb/blocks.cpp \ blocks.cpp diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp deleted file mode 100644 index f69b39d90..000000000 --- a/src/modules/controllib/uorb/UOrbPublication.cpp +++ /dev/null @@ -1,39 +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 UOrbPublication.cpp - * - */ - -#include "UOrbPublication.hpp" diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp deleted file mode 100644 index 6f1f3fc1c..000000000 --- a/src/modules/controllib/uorb/UOrbPublication.hpp +++ /dev/null @@ -1,118 +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 UOrbPublication.h - * - */ - -#pragma once - -#include -#include "../block/Block.hpp" -#include "../block/List.hpp" - - -namespace control -{ - -class Block; - -/** - * Base publication warapper class, used in list traversal - * of various publications. - */ -class __EXPORT UOrbPublicationBase : public ListNode -{ -public: - - UOrbPublicationBase( - List * list, - const struct orb_metadata *meta) : - _meta(meta), - _handle(-1) { - if (list != NULL) list->add(this); - } - void update() { - if (_handle > 0) { - orb_publish(getMeta(), getHandle(), getDataVoidPtr()); - } else { - setHandle(orb_advertise(getMeta(), getDataVoidPtr())); - } - } - virtual void *getDataVoidPtr() = 0; - virtual ~UOrbPublicationBase() { - orb_unsubscribe(getHandle()); - } - const struct orb_metadata *getMeta() { return _meta; } - int getHandle() { return _handle; } -protected: - void setHandle(orb_advert_t handle) { _handle = handle; } - const struct orb_metadata *_meta; - orb_advert_t _handle; -}; - -/** - * UOrb Publication wrapper class - */ -template -class UOrbPublication : - public T, // this must be first! - public UOrbPublicationBase -{ -public: - /** - * Constructor - * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - */ - UOrbPublication( - List * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - UOrbPublicationBase(list, meta) { - } - virtual ~UOrbPublication() {} - /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr() { return (void *)(T *)(this); } -}; - -} // namespace control diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp deleted file mode 100644 index 022cadd24..000000000 --- a/src/modules/controllib/uorb/UOrbSubscription.cpp +++ /dev/null @@ -1,51 +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 UOrbSubscription.cpp - * - */ - -#include "UOrbSubscription.hpp" - -namespace control -{ - -bool __EXPORT UOrbSubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} - -} // namespace control diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp deleted file mode 100644 index d337d89a8..000000000 --- a/src/modules/controllib/uorb/UOrbSubscription.hpp +++ /dev/null @@ -1,137 +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 UOrbSubscription.h - * - */ - -#pragma once - -#include -#include "../block/Block.hpp" -#include "../block/List.hpp" - - -namespace control -{ - -class Block; - -/** - * Base subscription warapper class, used in list traversal - * of various subscriptions. - */ -class __EXPORT UOrbSubscriptionBase : - public ListNode -{ -public: -// methods - - /** - * Constructor - * - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - */ - UOrbSubscriptionBase( - List * list, - const struct orb_metadata *meta) : - _meta(meta), - _handle() { - if (list != NULL) list->add(this); - } - bool updated(); - void update() { - if (updated()) { - orb_copy(_meta, _handle, getDataVoidPtr()); - } - } - virtual void *getDataVoidPtr() = 0; - virtual ~UOrbSubscriptionBase() { - orb_unsubscribe(_handle); - } -// accessors - const struct orb_metadata *getMeta() { return _meta; } - int getHandle() { return _handle; } -protected: -// accessors - void setHandle(int handle) { _handle = handle; } -// attributes - const struct orb_metadata *_meta; - int _handle; -}; - -/** - * UOrb Subscription wrapper class - */ -template -class __EXPORT UOrbSubscription : - public T, // this must be first! - public UOrbSubscriptionBase -{ -public: - /** - * Constructor - * - * @param list A list interface for adding to list during construction - * @param meta The uORB metadata (usually from the ORB_ID() macro) - * for the topic. - * @param interval The minimum interval in milliseconds between updates - */ - UOrbSubscription( - List * list, - const struct orb_metadata *meta, unsigned interval) : - T(), // initialize data structure to zero - UOrbSubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } - - /** - * Deconstructor - */ - virtual ~UOrbSubscription() {} - - /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr() { return (void *)(T *)(this); } - T getData() { return T(*this); } -}; - -} // namespace control diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 7c80c4b2b..a8a70507e 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -62,8 +62,8 @@ extern "C" { } #include "../blocks.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include +#include namespace control { @@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions - UOrbSubscription _att; - UOrbSubscription _attCmd; - UOrbSubscription _ratesCmd; - UOrbSubscription _pos; - UOrbSubscription _missionCmd; - UOrbSubscription _manual; - UOrbSubscription _status; - UOrbSubscription _param_update; + uORB::Subscription _att; + uORB::Subscription _attCmd; + uORB::Subscription _ratesCmd; + uORB::Subscription _pos; + uORB::Subscription _missionCmd; + uORB::Subscription _manual; + uORB::Subscription _status; + uORB::Subscription _param_update; // publications - UOrbPublication _actuators; + uORB::Publication _actuators; public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); virtual ~BlockUorbEnabledAutopilot(); diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp new file mode 100644 index 000000000..ed67b485d --- /dev/null +++ b/src/modules/uORB/Publication.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 Publication.cpp + * + */ + +#include "Publication.hpp" diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp new file mode 100644 index 000000000..7fa6bcc17 --- /dev/null +++ b/src/modules/uORB/Publication.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 Publication.h + * + */ + +#pragma once + +#include +#include + + +namespace uORB +{ + +/** + * Base publication warapper class, used in list traversal + * of various publications. + */ +class __EXPORT PublicationBase : public ListNode +{ +public: + + PublicationBase( + List * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle(-1) { + if (list != NULL) list->add(this); + } + void update() { + if (_handle > 0) { + orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + } else { + setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~PublicationBase() { + orb_unsubscribe(getHandle()); + } + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: + void setHandle(orb_advert_t handle) { _handle = handle; } + const struct orb_metadata *_meta; + orb_advert_t _handle; +}; + +/** + * Publication wrapper class + */ +template +class Publication : + public T, // this must be first! + public PublicationBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + Publication( + List * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + PublicationBase(list, meta) { + } + virtual ~Publication() {} + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } +}; + +} // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp new file mode 100644 index 000000000..6e8830708 --- /dev/null +++ b/src/modules/uORB/Subscription.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 Subscription.cpp + * + */ + +#include "Subscription.hpp" + +namespace uORB +{ + +bool __EXPORT SubscriptionBase::updated() +{ + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; +} + +} // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp new file mode 100644 index 000000000..55fb95d51 --- /dev/null +++ b/src/modules/uORB/Subscription.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 Subscription.h + * + */ + +#pragma once + +#include +#include + + +namespace uORB +{ + +/** + * Base subscription warapper class, used in list traversal + * of various subscriptions. + */ +class __EXPORT SubscriptionBase : + public ListNode +{ +public: +// methods + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + SubscriptionBase( + List * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle() { + if (list != NULL) list->add(this); + } + bool updated(); + void update() { + if (updated()) { + orb_copy(_meta, _handle, getDataVoidPtr()); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~SubscriptionBase() { + orb_unsubscribe(_handle); + } +// accessors + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: +// accessors + void setHandle(int handle) { _handle = handle; } +// attributes + const struct orb_metadata *_meta; + int _handle; +}; + +/** + * Subscription wrapper class + */ +template +class __EXPORT Subscription : + public T, // this must be first! + public SubscriptionBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param interval The minimum interval in milliseconds between updates + */ + Subscription( + List * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + SubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); + } + + /** + * Deconstructor + */ + virtual ~Subscription() {} + + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } + T getData() { return T(*this); } +}; + +} // namespace uORB diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 5ec31ab01..0c29101fe 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -41,4 +41,6 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 4096 SRCS = uORB.cpp \ - objects_common.cpp + objects_common.cpp \ + Publication.cpp \ + Subscription.cpp -- cgit v1.2.3 From da9dab27998fa9e8b7a66d53a00aa3cae93573ec Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 16:21:06 -0400 Subject: Moved List.hpp from controllib to src/include/containers. --- src/include/containers/List.hpp | 71 +++++++++++++++++++++++++++++ src/modules/controllib/block/Block.hpp | 2 +- src/modules/controllib/block/BlockParam.hpp | 2 +- src/modules/controllib/block/List.hpp | 71 ----------------------------- src/modules/uORB/Publication.hpp | 2 +- src/modules/uORB/Subscription.hpp | 2 +- 6 files changed, 75 insertions(+), 75 deletions(-) create mode 100644 src/include/containers/List.hpp delete mode 100644 src/modules/controllib/block/List.hpp diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp new file mode 100644 index 000000000..13cbda938 --- /dev/null +++ b/src/include/containers/List.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 List.hpp + * + * A linked list. + */ + +#pragma once + +template +class __EXPORT ListNode +{ +public: + ListNode() : _sibling(nullptr) { + } + void setSibling(T sibling) { _sibling = sibling; } + T getSibling() { return _sibling; } + T get() { + return _sibling; + } +protected: + T _sibling; +}; + +template +class __EXPORT List +{ +public: + List() : _head() { + } + void add(T newNode) { + newNode->setSibling(getHead()); + setHead(newNode); + } + T getHead() { return _head; } +private: + void setHead(T &head) { _head = head; } + T _head; +}; diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 6a8e1b5b9..736698e21 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -42,7 +42,7 @@ #include #include -#include "List.hpp" +#include // forward declaration namespace uORB { diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 735b2b292..a64d0139e 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -42,7 +42,7 @@ #include #include "Block.hpp" -#include "List.hpp" +#include namespace control { diff --git a/src/modules/controllib/block/List.hpp b/src/modules/controllib/block/List.hpp deleted file mode 100644 index 96b0b94d1..000000000 --- a/src/modules/controllib/block/List.hpp +++ /dev/null @@ -1,71 +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 Node.h - * - * A node of a linked list. - */ - -#pragma once - -template -class __EXPORT ListNode -{ -public: - ListNode() : _sibling(NULL) { - } - void setSibling(T sibling) { _sibling = sibling; } - T getSibling() { return _sibling; } - T get() { - return _sibling; - } -protected: - T _sibling; -}; - -template -class __EXPORT List -{ -public: - List() : _head() { - } - void add(T newNode) { - newNode->setSibling(getHead()); - setHead(newNode); - } - T getHead() { return _head; } -private: - void setHead(T &head) { _head = head; } - T _head; -}; diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 7fa6bcc17..21840ed70 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 55fb95d51..d839f8023 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace uORB -- cgit v1.2.3 From afb2c37bfc20150718114c4ef56e40a7c4d4722f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:01:03 -0400 Subject: Fixed uORB Pub/Sub templates for GCC 4.7 --- src/modules/uORB/Publication.cpp | 27 +++++++++++++++++++++++++++ src/modules/uORB/Publication.hpp | 12 ++++-------- src/modules/uORB/Subscription.cpp | 32 ++++++++++++++++++++++++++++++++ src/modules/uORB/Subscription.hpp | 14 ++++---------- 4 files changed, 67 insertions(+), 18 deletions(-) diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index ed67b485d..78a5cd204 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -37,3 +37,30 @@ */ #include "Publication.hpp" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_global_position.h" + +namespace uORB { + +template +Publication::Publication( + List * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + PublicationBase(list, meta) { +} + +template +Publication::~Publication() {} + +template +void * Publication::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; + +} diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 21840ed70..8650b3df8 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -95,13 +95,9 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - Publication( - List * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - PublicationBase(list, meta) { - } - virtual ~Publication() {} + Publication(List * list, + const struct orb_metadata *meta); + virtual ~Publication(); /* * XXX * This function gets the T struct, assuming @@ -109,7 +105,7 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } + void *getDataVoidPtr(); }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 6e8830708..16da3b66f 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -37,6 +37,10 @@ */ #include "Subscription.hpp" +#include "topics/parameter_update.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_gps_position.h" +#include "topics/sensor_combined.h" namespace uORB { @@ -48,4 +52,32 @@ bool __EXPORT SubscriptionBase::updated() return isUpdated; } +template +Subscription::Subscription( + List * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + SubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); +} + +template +Subscription::~Subscription() {} + +template +void * Subscription::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template +T Subscription::getData() { + return T(*this); +} + +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; + } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index d839f8023..34e9a83e0 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -108,17 +108,11 @@ public: */ Subscription( List * list, - const struct orb_metadata *meta, unsigned interval) : - T(), // initialize data structure to zero - SubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } - + const struct orb_metadata *meta, unsigned interval); /** * Deconstructor */ - virtual ~Subscription() {} + virtual ~Subscription(); /* * XXX @@ -127,8 +121,8 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } - T getData() { return T(*this); } + void *getDataVoidPtr(); + T getData(); }; } // namespace uORB -- cgit v1.2.3 From 1b7472ef4c009caa8e76d6a4d90979677f14b0f4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:06:37 -0400 Subject: Fix for md25 and uORB update. --- src/drivers/md25/md25.cpp | 2 +- src/modules/uORB/Publication.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 6d5e805ea..5d1f58b85 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 78a5cd204..dbd56e642 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -40,6 +40,7 @@ #include "topics/vehicle_attitude.h" #include "topics/vehicle_local_position.h" #include "topics/vehicle_global_position.h" +#include "topics/debug_key_value.h" namespace uORB { @@ -62,5 +63,6 @@ void * Publication::getDataVoidPtr() { template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } -- cgit v1.2.3 From 8f0b223c874b33bf09a8ceebcaf40cd60ee3800d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:16:30 -0400 Subject: Fixed include style for List in Block. --- src/modules/controllib/block/Block.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 0b4cce979..6768bfa7e 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -41,10 +41,11 @@ #include #include +#include +#include + #include "Block.hpp" #include "BlockParam.hpp" -#include "uORB/Subscription.hpp" -#include "uORB/Publication.hpp" namespace control { -- cgit v1.2.3 From eaef67f21df3d38a6523a79bb966fe62d44092ae Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 18:35:26 -0400 Subject: Added encoder uORB message/ fixedwing_backside working if enabled. --- src/modules/fixedwing_backside/fixedwing.cpp | 6 +-- src/modules/uORB/Publication.cpp | 12 +++++ src/modules/uORB/Subscription.cpp | 20 +++++++++ src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/encoders.h | 66 ++++++++++++++++++++++++++++ 5 files changed, 104 insertions(+), 3 deletions(-) create mode 100644 src/modules/uORB/topics/encoders.h diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f7c0b6148..cfae07275 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); @@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index dbd56e642..5a5981617 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -41,6 +41,12 @@ #include "topics/vehicle_local_position.h" #include "topics/vehicle_global_position.h" #include "topics/debug_key_value.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_global_velocity_setpoint.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" +#include "topics/actuator_outputs.h" +#include "topics/encoders.h" namespace uORB { @@ -64,5 +70,11 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 16da3b66f..c1d1a938f 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -41,6 +41,16 @@ #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" #include "topics/sensor_combined.h" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_global_position.h" +#include "topics/encoders.h" +#include "topics/position_setpoint_triplet.h" +#include "topics/vehicle_status.h" +#include "topics/manual_control_setpoint.h" +#include "topics/vehicle_local_position_setpoint.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" namespace uORB { @@ -79,5 +89,15 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4c84c1f25..fb24de8d1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); #include "topics/esc_status.h" ORB_DEFINE(esc_status, struct esc_status_s); + +#include "topics/encoders.h" +ORB_DEFINE(encoders, struct encoders_s); diff --git a/src/modules/uORB/topics/encoders.h b/src/modules/uORB/topics/encoders.h new file mode 100644 index 000000000..588c0ddb1 --- /dev/null +++ b/src/modules/uORB/topics/encoders.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 encoders.h + * + * Encoders topic. + * + */ + +#ifndef TOPIC_ENCODERS_H +#define TOPIC_ENCODERS_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +#define NUM_ENCODERS 4 + +struct encoders_s { + uint64_t timestamp; + int64_t counts[NUM_ENCODERS]; // counts of encoder + float velocity[NUM_ENCODERS]; // counts of encoder/ second +}; + +/** + * @} + */ + +ORB_DECLARE(encoders); + +#endif -- cgit v1.2.3 From 98ef3b9ab882da077c67f81eb2f7852c44badb36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:07:56 +0100 Subject: Make driver start less verbose --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - src/drivers/meas_airspeed/meas_airspeed.cpp | 13 ++++--------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b6e80ce1d..e77f31e87 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,7 +132,6 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); } return ret; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7baa8738f..4f64d8585 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -169,18 +169,13 @@ MEASAirspeed::collect() uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { - case 0: break; + case 0: + break; case 1: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 2: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); -- cgit v1.2.3 From 8af83f70747d104511cc100e4f94ff4d5c6ac6d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:11:28 +0100 Subject: Make error message less dramatic, as its part of a scanning progress --- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index e77f31e87..d27ab9727 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -307,7 +307,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no ETS airspeed sensor connected"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 4f64d8585..beca28e7a 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -356,7 +356,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no MS4525 airspeed sensor connected"); } /** -- cgit v1.2.3 From b8afcf5863fd59d740fd81a77cfc97cc9ecdc07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:32:54 +0100 Subject: Ensure that the mavlink start call only returns once the new instance is fully initialized. This avoids race conditions in getopt() and it ensures that the mavlink debug fd is ready when other processes start --- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9c2e0178a..c9f4835ba 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -254,7 +254,6 @@ Mavlink::Mavlink() : break; } - LL_APPEND(_mavlink_instances, this); } Mavlink::~Mavlink() @@ -1778,6 +1777,9 @@ Mavlink::task_main(int argc, char *argv[]) /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + /* now the instance is fully initialized and we can bump the instance count */ + LL_APPEND(_mavlink_instances, this); + while (!_task_should_exit) { /* main loop */ usleep(_main_loop_delay); @@ -1924,10 +1926,18 @@ int Mavlink::start_helper(int argc, char *argv[]) int Mavlink::start(int argc, char *argv[]) { + // Wait for the instance count to go up one + // before returning to the shell + int ic = Mavlink::instance_count(); + // Instantiate thread char buf[24]; - sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + sprintf(buf, "mavlink_if%d", ic); + // This is where the control flow splits + // between the starting task and the spawned + // task - start_helper() only returns + // when the started task exits. task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, @@ -1935,6 +1945,16 @@ Mavlink::start(int argc, char *argv[]) (main_t)&Mavlink::start_helper, (const char **)argv); + // Ensure that this shell command + // does not return before the instance + // is fully initialized. As this is also + // the only path to create a new instance, + // this is effectively a lock on concurrent + // instance starting. XXX do a real lock. + while (ic == Mavlink::instance_count()) { + ::usleep(500); + } + return OK; } -- cgit v1.2.3 From 055e45935534b409cd3ed68c837399461e448b3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:33:49 +0100 Subject: Remove unneeded headers --- src/modules/mavlink/mavlink.c | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 542bf0d7e..ad435b251 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -45,31 +45,6 @@ #include #include "mavlink_bridge_header.h" #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 /* define MAVLink specific parameters */ /** -- cgit v1.2.3 From 2988136e7eaa1f55c41376b74b58766af9f8fcb9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 10:44:31 +0100 Subject: Implemented last missing messages, added stream config for USB, made stream config fails for non-existing mavlink links non-fatal --- ROMFS/px4fmu_common/init.d/rc.usb | 4 + src/modules/mavlink/mavlink_main.cpp | 21 ++++- src/modules/mavlink/mavlink_messages.cpp | 156 ++++++++++++++++++------------- 3 files changed, 110 insertions(+), 71 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be4275..2af790fc4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -6,6 +6,10 @@ echo "Starting MAVLink on this USB console" mavlink start -r 10000 -d /dev/ttyACM0 +# Enable a number of interesting streams we want via USB +mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 +mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROL -r 30 # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c9f4835ba..00d906dcd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -588,7 +588,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* setup output flow control */ if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + warnx("hardware flow control not supported"); } } @@ -1762,6 +1762,7 @@ Mavlink::task_main(int argc, char *argv[]) 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); + configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); break; default: @@ -1951,8 +1952,17 @@ Mavlink::start(int argc, char *argv[]) // the only path to create a new instance, // this is effectively a lock on concurrent // instance starting. XXX do a real lock. - while (ic == Mavlink::instance_count()) { - ::usleep(500); + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; } return OK; @@ -2008,7 +2018,10 @@ Mavlink::stream(int argc, char *argv[]) inst->configure_stream_threadsafe(stream_name, rate); } else { - errx(1, "mavlink for device %s is not running", device_name); + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + errx(0, "mavlink for device %s is not running", device_name); } } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88d..dc01935ca 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1127,6 +1127,93 @@ protected: } }; +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + struct actuator_controls_s *att_ctrl; + +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (att_ctrl_sub->update(t)) { + + // send, add spaces so that string buffer is at least 10 chars long + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); + } + } +}; + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() + { + return "NAMED_VALUE_FLOAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + struct debug_key_value_s *debug; + +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + debug = (struct debug_key_value_s *)debug_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (debug_sub->update(t)) { + + // Enforce null termination + debug->key[sizeof(debug->key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(_channel, + debug->timestamp_ms, + debug->key, + debug->value); + } + } +}; MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,72 +1238,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamAttitudeControls(), + new MavlinkStreamNamedValueFloat(), nullptr }; - - - - - - - -// -// -// -// -// -// -//void -//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -//{ -// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl0 ", -// l->listener->actuators_0.control[0]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl1 ", -// l->listener->actuators_0.control[1]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl2 ", -// l->listener->actuators_0.control[2]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl3 ", -// l->listener->actuators_0.control[3]); -// } -//} -// -//void -//MavlinkOrbListener::l_debug_key_value(const struct listener *l) -//{ -// struct debug_key_value_s debug; -// -// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); -// -// /* Enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// debug.key, -// debug.value); -//} -// -//void -//MavlinkOrbListener::l_nav_cap(const struct listener *l) -//{ -// -// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// hrt_absolute_time() / 1000, -// "turn dist", -// l->listener->nav_cap.turn_distance); -// -//} -- cgit v1.2.3 From 057bcf3172f3c8d1bab561e7e4cad14977cd74d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 10:45:12 +0100 Subject: Changed RC scaling for fixed wing defaults --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56..4cd73e23f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,4 +11,6 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_PITCH 1 fi \ No newline at end of file -- cgit v1.2.3 From b2f8fcb9d904901127b937551c149e904c8aa3a9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 18:49:39 +0400 Subject: position_estimator_inav: added NaN checks --- .../position_estimator_inav/inertial_filter.c | 24 ++++++++++++++-------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 7cd076948..2f1b3c014 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -5,24 +5,30 @@ * Author: Anton Babushkin */ +#include + #include "inertial_filter.h" void inertial_filter_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (isfinite(dt)) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; + } } void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = e * w * dt; - x[i] += ewdt; + if (isfinite(e) && isfinite(w) && isfinite(dt)) { + float ewdt = e * w * dt; + x[i] += ewdt; - if (i == 0) { - x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; + if (i == 0) { + x[1] += w * ewdt; + x[2] += w * w * ewdt / 3.0; - } else if (i == 1) { - x[2] += w * ewdt; + } else if (i == 1) { + x[2] += w * ewdt; + } } } -- cgit v1.2.3 From b1e59f37fe3f652002f2e92a205de5994c9c8120 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 19:48:53 +0400 Subject: rc.usb: typo fixed --- 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 2af790fc4..2aeea5cbe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -9,7 +9,7 @@ mavlink start -r 10000 -d /dev/ttyACM0 # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROL -r 30 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 # Exit shell to make it available to MAVLink exit -- cgit v1.2.3 From 1c49d768fbf2b69bf2500a4dc48ac2a95c0d2ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 20:06:34 +0400 Subject: mavlink: code style fixed --- src/modules/mavlink/mavlink_main.cpp | 41 +++++++++++---- src/modules/mavlink/mavlink_messages.cpp | 90 +++++++++++++------------------- 2 files changed, 68 insertions(+), 63 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ff1c070cb..9a8e1e5a7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } /* no valid instance, bail */ - if (!instance) + if (!instance) { return; + } int uart = instance->get_uart_fd(); @@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && 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) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -225,30 +226,37 @@ Mavlink::Mavlink() : 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: + + 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; @@ -279,6 +287,7 @@ Mavlink::~Mavlink() } } while (_task_running); } + LL_DELETE(_mavlink_instances, this); } @@ -604,12 +613,16 @@ 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); if (!ret) { @@ -1652,6 +1665,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + } else if (strcmp(optarg, "camera") == 0) { _mode = MAVLINK_MODE_CAMERA; } @@ -1973,6 +1987,7 @@ Mavlink::start(int argc, char *argv[]) const unsigned limit = 100 * 1000 / sleeptime; unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { ::usleep(sleeptime); count++; @@ -2003,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[]) bool err_flag = false; int i = 0; + while (i < argc) { - if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { - rate = strtod(argv[i+1], nullptr); + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + if (rate < 0.0f) { err_flag = true; } + i++; - } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { - device_name = argv[i+1]; + + } 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]; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; i++; + } else { err_flag = true; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4a3c681f..037999af7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,7 +262,6 @@ protected: void send(const hrt_abstime t) { if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, status->onboard_control_sensors_present, status->onboard_control_sensors_enabled, @@ -318,7 +317,6 @@ protected: void send(const hrt_abstime t) { if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; if (accel_timestamp != sensor->accelerometer_timestamp) { @@ -385,7 +383,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, att->timestamp / 1000, att->roll, att->pitch, att->yaw, @@ -422,7 +419,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_quaternion_send(_channel, att->timestamp / 1000, att->q[0], @@ -494,7 +490,6 @@ protected: updated |= airspeed_sub->update(t); if (updated) { - 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; @@ -538,7 +533,6 @@ protected: void send(const hrt_abstime t) { if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, gps->timestamp_position, gps->fix_type, @@ -592,15 +586,15 @@ protected: if (updated) { 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); } } }; @@ -633,7 +627,6 @@ protected: void send(const hrt_abstime t) { if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, pos->timestamp / 1000, pos->x, @@ -730,7 +723,6 @@ protected: void send(const hrt_abstime t) { if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, act->timestamp / 1000, _n, @@ -790,7 +782,6 @@ protected: updated |= act_sub->update(t); if (updated) { - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -870,7 +861,6 @@ protected: void send(const hrt_abstime 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), @@ -908,14 +898,14 @@ protected: 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); + if (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); + } } }; @@ -947,7 +937,6 @@ protected: void send(const hrt_abstime t) { if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, att_sp->timestamp / 1000, att_sp->roll_body, @@ -986,7 +975,6 @@ protected: void send(const hrt_abstime 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, @@ -1025,7 +1013,6 @@ protected: void send(const hrt_abstime t) { if (rc_sub->update(t)) { - const unsigned port_width = 8; for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { @@ -1075,7 +1062,6 @@ protected: void send(const hrt_abstime t) { if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, manual->roll * 1000, @@ -1115,7 +1101,6 @@ protected: void send(const hrt_abstime t) { if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, flow->timestamp, flow->sensor_id, @@ -1154,24 +1139,23 @@ protected: void send(const hrt_abstime t) { if (att_ctrl_sub->update(t)) { - - // send, add spaces so that string buffer is at least 10 chars long - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "rll ctrl ", - att_ctrl->control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "ptch ctrl ", - att_ctrl->control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "yaw ctrl ", - att_ctrl->control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "thr ctrl ", - att_ctrl->control[3]); + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); } } }; @@ -1203,8 +1187,7 @@ protected: void send(const hrt_abstime t) { if (debug_sub->update(t)) { - - // Enforce null termination + /* enforce null termination */ debug->key[sizeof(debug->key) - 1] = '\0'; mavlink_msg_named_value_float_send(_channel, @@ -1246,10 +1229,11 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + } else { /* send camera capture off */ mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -- cgit v1.2.3 From 7fa6b48a36d80ed0b0b3b81cebf45810c0d6f42d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 21 Mar 2014 17:51:46 +0100 Subject: wingwing startup script: add FW_THR_CRUISE param --- ROMFS/px4fmu_common/init.d/3033_wingwing | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 22d63fad5..7ff381d97 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -29,6 +29,7 @@ then param set FW_RR_FF 0.6 param set FW_RR_IMA 0.2 param set FW_RR_P 0.09 + param set FW_THR_CRUISE 0.65 fi set MIXER FMU_Q -- cgit v1.2.3 From 69bc0c8e3998f87358bbf8a5f97f5c31dca43f76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 18:12:02 +0100 Subject: Reduced param rate - as long as we do not have proper QoS (= reducing all rates at once if link becomes lossy) we need to open-loop match the link --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a8e1e5a7..18df577fe 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1800,7 +1800,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(20000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; -- cgit v1.2.3 From 5f5a200f4a46ed63a96be4011249d94aa312401b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 22 Mar 2014 10:09:30 +0100 Subject: Add support for the SkyHunter 1800 --- ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 | 5 +++++ ROMFS/px4fmu_common/init.d/rc.autostart | 6 ++++++ 2 files changed, 11 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 diff --git a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 new file mode 100644 index 000000000..9bc0262d8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 @@ -0,0 +1,5 @@ +#!nsh + +sh /etc/init.d/rc.fw_defaults + +set MIXER FMU_AET diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 4ae6a2794..5ec735d39 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -68,6 +68,12 @@ then set MODE custom fi +if param compare SYS_AUTOSTART 2103 103 +then + sh /etc/init.d/2103_skyhunter_1800 + set MODE custom +fi + # # Flying wing # -- cgit v1.2.3 From 6b610dc1f88eab46cbfe9c297256d2ac6aa81baf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Mar 2014 10:44:14 +0100 Subject: Hotfix for comment: Removed syntax-breaking symbol --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0909135e1..37f06dbe5 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 @@ -245,7 +245,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); /** * Maximum vertical acceleration * - * This is the maximum vertical acceleration (in metres/second^2) + * This is the maximum vertical acceleration (in metres/second square) * either up or down that the controller will use to correct speed * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) * allows for reasonably aggressive pitch changes if required to recover -- cgit v1.2.3