From 1fd7b89d3c7c5f9b5c13198c41a015d6c5899260 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:03:49 +0400 Subject: [PATCH 01/19] 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 5bdf6c2622..e1c53a5993 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -165,7 +165,7 @@ Mavlink::Mavlink() : _task_should_exit(false), _mavlink_fd(-1), _task_running(false), - _mavlink_hil_enabled(false), + _hil_enabled(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -548,38 +548,17 @@ Mavlink::set_hil_enabled(bool hil_enabled) { int ret = OK; - /* Enable HIL */ - if (hil_enabled && !_mavlink_hil_enabled) { - - _mavlink_hil_enabled = true; - - /* ramp up some HIL-related subscriptions */ - unsigned hil_rate_interval; - - if (_baudrate < 19200) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (_baudrate < 38400) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (_baudrate < 115200) { - /* 20 Hz */ - hil_rate_interval = 50; - - } else { - /* 200 Hz */ - hil_rate_interval = 5; - } - -// orb_set_interval(subs.spa_sub, hil_rate_interval); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + /* enable HIL */ + if (hil_enabled && !_hil_enabled) { + _hil_enabled = true; + float rate_mult = _datarate / 1000.0f; + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); } - if (!hil_enabled && _mavlink_hil_enabled) { - _mavlink_hil_enabled = false; -// orb_set_interval(subs.spa_sub, 200); + /* disable HIL */ + if (!hil_enabled && _hil_enabled) { + _hil_enabled = false; + configure_stream("HIL_CONTROLS", 0.0f); } else { ret = ERROR; @@ -1445,10 +1424,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) if (i > 1) { /* Enforce null termination */ statustext.text[i] = '\0'; - mavlink_message_t msg; - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - mavlink_missionlib_send_message(&msg); + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; } else { @@ -1560,9 +1537,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; _datarate = 0; - _channel = MAVLINK_COMM_0; - - _mode = MODE_OFFBOARD; + _mode = MAVLINK_MODE_NORMAL; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -1603,17 +1578,8 @@ Mavlink::task_main(int argc, char *argv[]) // break; case 'm': - if (strcmp(optarg, "offboard") == 0) { - _mode = MODE_OFFBOARD; - - } else if (strcmp(optarg, "onboard") == 0) { - _mode = MODE_ONBOARD; - - } else if (strcmp(optarg, "hil") == 0) { - _mode = MODE_HIL; - - } else if (strcmp(optarg, "custom") == 0) { - _mode = MODE_CUSTOM; + if (strcmp(optarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; } break; @@ -1649,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 cc4c896b91..3a8625c6bf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -145,16 +145,14 @@ public: const char *_device_name; enum MAVLINK_MODE { - MODE_CUSTOM = 0, - MODE_OFFBOARD, - MODE_ONBOARD, - MODE_HIL + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM }; void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _mavlink_hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; }; /** * Handle waypoint related messages. @@ -200,7 +198,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* states */ - bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + bool _hil_enabled; /**< Hardware In the Loop mode */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From 4cee3614c7bc2e960ac52e59014bc4d08b8da11e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:04:23 +0400 Subject: [PATCH 02/19] rc.usb: increase data rate to 10000bytes/s for USB --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0de2d42958..558be42755 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -r 5000 -d /dev/ttyACM0 +mavlink start -r 10000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit From 5f847ff8a3df1ba303129a7a4938305a6901755c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 21:14:54 +0100 Subject: [PATCH 03/19] Remove HIL flag on startup --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index eba18ddb19..8cb8f4dc77 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -392,7 +392,7 @@ then if [ $HIL == yes ] then sleep 1 - set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From 15c079921bf2f8256e0fcdd5ab9f31157bc09f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:12:03 +0100 Subject: [PATCH 04/19] Fixed missing parent ioctl call --- src/drivers/gps/gps.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a736cbdf6b..785c8beeb0 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,10 +229,15 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); + case SENSORIOCRESET: + cmd_reset(); + break; + } + + default: + /* give it to parent if no one wants it */ + ret = CDev::ioctl(filp, cmd, arg); break; - } unlock(); From 85b7670b44d0af1141fae73593f68d8469846d8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:15:31 +0100 Subject: [PATCH 05/19] compile fix --- src/drivers/gps/gps.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 785c8beeb0..c72f692d73 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,15 +229,15 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); - break; - } + case SENSORIOCRESET: + cmd_reset(); + break; - default: + default: /* give it to parent if no one wants it */ ret = CDev::ioctl(filp, cmd, arg); break; + } unlock(); From 8818425e584c9670dfa9a823b3dfd95147c087d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 18:25:09 +0400 Subject: [PATCH 06/19] commander: fixed message formatting when disabling sensors publishing in HIL mode --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5d..bf49403d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -350,7 +350,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); From bb7962936e15c594821ae10e4510febfec0d6b9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:37:16 +0100 Subject: [PATCH 07/19] CDev: Fixed printf format specifier --- src/drivers/device/cdev.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index b157b3f181..6e2ebb1f7b 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname) if (ret == OK) break; } else { char name[32]; - snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); if (ret == OK) break; } From 772fce9f82749bde8c14199fcc73cfa2235e760e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:38:55 +0100 Subject: [PATCH 08/19] commander: Improve HIL publication blocking call handling --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5d..131d6a550d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -309,10 +309,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s bool valid_transition = false; int ret = ERROR; - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); valid_transition = true; } else { @@ -347,10 +344,16 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + + if (sensfd < 0) { + warn("failed opening device"); + return 1; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "FAIL"); } closedir(d); From 8383603f7691e3bc66534b58654c4d13547632a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:41:03 +0100 Subject: [PATCH 09/19] commander: Linting: Reduce scope of variable --- src/modules/commander/state_machine_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 131d6a550d..5537c4e801 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -337,10 +337,11 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; - struct dirent *direntry; d = opendir("/dev"); if (d) { + struct dirent *direntry; + while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); From 6db2191a71ed0ea38e1fd18886371f099c14d593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:50:20 +0100 Subject: [PATCH 10/19] commander: Skip devices we do not want to touch in HIL --- .../commander/state_machine_helper.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1e892ed77f..9178bcc5ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -351,6 +351,23 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s return 1; } + /* skip serial ports */ + if (!strncmp("tty", direntry-d_name, 3)) { + continue; + } + /* skip mtd devices */ + if (!strncmp("mtd", direntry-d_name, 3)) { + continue; + } + /* skip ram devices */ + if (!strncmp("ram", direntry-d_name, 3)) { + continue; + } + /* skip mavlink */ + if (!strcmp("mavlink", direntry-d_name)) { + continue; + } + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); @@ -359,8 +376,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s closedir(d); - warnx("directory listing ok (FS mounted and readable)"); - } else { /* failed opening dir */ warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); From 2fe9f65c45ba3aaad549b30e3b34568d07ab2b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:16:18 +0100 Subject: [PATCH 11/19] commander: Fixed compile error and some stupidity in usage of path names --- .../commander/state_machine_helper.cpp | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9178bcc5ab..e6f245cf93 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -341,37 +342,52 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (d) { struct dirent *direntry; + char devname[24]; while ((direntry = readdir(d)) != NULL) { - int sensfd = ::open(direntry->d_name, 0); - - if (sensfd < 0) { - warn("failed opening device"); - return 1; - } - /* skip serial ports */ - if (!strncmp("tty", direntry-d_name, 3)) { + if (!strncmp("tty", direntry->d_name, 3)) { continue; } /* skip mtd devices */ - if (!strncmp("mtd", direntry-d_name, 3)) { + if (!strncmp("mtd", direntry->d_name, 3)) { continue; } /* skip ram devices */ - if (!strncmp("ram", direntry-d_name, 3)) { + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { continue; } /* skip mavlink */ - if (!strcmp("mavlink", direntry-d_name)) { + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); continue; } int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); From 2db7d30400d0240baf4532eeef6cb13403417654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:43:09 +0100 Subject: [PATCH 12/19] 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 11882f0f40..9c2e0178a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -201,6 +201,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _is_usb_uart(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -1; } - /* - * Setup hardware flow control. If the port has no RTS pin this call will fail, - * which is not an issue, but requires a separate call so we can fail silently. - */ - (void)tcgetattr(_uart_fd, &uart_config); - uart_config.c_cflag |= CRTS_IFLOW; - (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); - - /* 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 e6e20eb938..f743c25044 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -208,6 +208,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _is_usb_uart; /**< Port is USB */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From ffd0d1032041601916e2bd2faa395ca0af03659c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:25:22 +0100 Subject: [PATCH 13/19] Fix usage of right time stamps --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 11ae2a2683..10a6cd2c5c 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -393,7 +393,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; @@ -445,7 +445,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 7150218ffc..3653defa66 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -539,7 +539,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,7 +550,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; From 8cb5a12cc702ac238f4dc79bf1f1cee3fdee005f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:23 +0100 Subject: [PATCH 14/19] Remove now unused hil_counter --- src/modules/mavlink/mavlink_receiver.cpp | 3 --- src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2ce86396cf..71ea832fdd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,8 +108,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - - _hil_counter(0), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -647,7 +645,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) } /* increment counters */ - _hil_counter++; _hil_frames++; /* print HIL sensors rate */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index beaae20587..0a5a1b5c7a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -139,7 +139,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - int _hil_counter; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; From dbd467fe1f3779b45090f556fd05b056f068cd34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:49 +0100 Subject: [PATCH 15/19] sensors: Remove effect-less writing of timestamp - was anyway correctly written by gyro update --- src/modules/sensors/sensors.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 76dc262f13..0ed26b54cb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1592,8 +1592,7 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); - /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ - raw.timestamp = hrt_absolute_time(); + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); From 533e3172dc37aeeb2bc4ccc4ee884239d91079f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:24:31 +0100 Subject: [PATCH 16/19] Make PX4IO driver obey HIL as it should --- src/drivers/px4io/px4io.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8e990aa6f0..82f3ba044a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1959,8 +1962,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; From 9cdb416d3a6ceb1c74b03f3f046f5d904bf7fc45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:32:02 +0100 Subject: [PATCH 17/19] Teach RGB led driver to forward unknown IOCTLs --- src/drivers/rgbled/rgbled.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4f58891ed3..e390140de3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); break; } From 75ad1c4a1350d73315f48a1f4b9bdbee2d20fa9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:35:07 +0100 Subject: [PATCH 18/19] Completely and properly populate battery status message in HIL --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 71ea832fdd..489d2bdcb7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -851,7 +851,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); From 51658ac857390f20d3cc9e2e59a485516c4e0814 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:37:32 +0100 Subject: [PATCH 19/19] Compile fix --- src/drivers/rgbled/rgbled.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index e390140de3..13cbfdfa8b 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -243,7 +243,7 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* see if the parent class can make any use of it */ - ret = CDev::ioctl(filep, cmd, arg); + ret = CDev::ioctl(filp, cmd, arg); break; }