From 2af066bc6aa5c3fe5b4e9a676c31be6462c995fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Dec 2015 11:14:03 +0100 Subject: [PATCH 01/45] guard against invalid yawmode values --- src/modules/navigator/mission.cpp | 3 ++- src/modules/navigator/mission.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 299b69b3d0..ab8e43add4 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -189,8 +189,9 @@ Mission::on_active() } /* see if we need to update the current yaw heading for rotary wing types */ - if (_navigator->get_vstatus()->is_rotary_wing + if (_navigator->get_vstatus()->is_rotary_wing && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index b0117cf1fe..c8a34132de 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -88,7 +88,8 @@ public: MISSION_YAWMODE_NONE = 0, MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, MISSION_YAWMODE_FRONT_TO_HOME = 2, - MISSION_YAWMODE_BACK_TO_HOME = 3 + MISSION_YAWMODE_BACK_TO_HOME = 3, + MISSION_YAWMODE_MAX = 4 }; private: From 7c52e8b96f132db3dabed275517f17225e3ea1e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Dec 2015 11:15:03 +0100 Subject: [PATCH 02/45] Improve landing for special yawmode settings --- src/modules/navigator/mission.cpp | 64 +++++++++++++++++++------------ 1 file changed, 40 insertions(+), 24 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ab8e43add4..55628f3438 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -526,36 +526,52 @@ Mission::heading_sp_update() return; } - /* Don't do FOH for landing and takeoff waypoints, the ground may be near - * and the FW controller has a custom landing logic */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* Don't change heading for takeoff waypoints, the ground may be near */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { return; } /* set yaw angle for the waypoint iff a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { _mission_item.yaw = _on_arrival_yaw; - /* always keep the front of the rotary wing pointing to the next waypoint */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _mission_item.lat, - _mission_item.lon); - /* always keep the back of the rotary wing pointing towards home */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon); - /* always keep the back of the rotary wing pointing towards home */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { - _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon) + M_PI_F); + } else { + + /* calculate direction the vehicle should point to: + * normal waypoint: current position to {waypoint or home or home + 180deg} + * landing waypoint: last waypoint to {waypoint or home or home + 180deg} + * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground + */ + double point_from_latlon[2]; + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + point_from_latlon[0] = pos_sp_triplet->previous.lat; + point_from_latlon[1] = pos_sp_triplet->previous.lon; + } else { + point_from_latlon[0] = _navigator->get_global_position()->lat; + point_from_latlon[1] = _navigator->get_global_position()->lon; + } + + /* always keep the front of the rotary wing pointing to the next waypoint */ + if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } } mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); From 6e1f54e2ff62d9cf7d5f2d7044d976f0308d7d2d Mon Sep 17 00:00:00 2001 From: lchish Date: Thu, 22 Oct 2015 09:22:55 +1300 Subject: [PATCH 03/45] Add missing stm32f4discovery make targets --- .travis.yml | 6 +- Makefile | 3 + .../nuttx_px4-stm32f4discovery_default.cmake | 87 +++++++++++++++++++ cmake/nuttx/px4_impl_nuttx.cmake | 8 ++ .../px4-stm32f4discovery/px4discovery_init.c | 12 --- src/firmware/nuttx/CMakeLists.txt | 2 +- 6 files changed, 103 insertions(+), 15 deletions(-) create mode 100644 cmake/configs/nuttx_px4-stm32f4discovery_default.cmake diff --git a/.travis.yml b/.travis.yml index ccd0aafbcd..d984d2a83e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -110,6 +110,7 @@ script: - ../Tools/uavcan_copy.sh - cd .. - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default + - echo 'Building NuttX px4-stm32f4discovery Firmware..' && make px4-stm32f4discovery_default - echo 'Running Tests..' && make px4fmu-v2_default test after_success: @@ -117,8 +118,9 @@ after_success: cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 && cp build_px4fmu-v4_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4 - && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 - && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && cp build_px4-stm32f4discovery_default/src/firmware/nuttx/nuttx-px4-stm32f4discovery-default.px4 px4-stm32f4discovery-default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4 + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html diff --git a/Makefile b/Makefile index ee2670244d..77abd708a6 100644 --- a/Makefile +++ b/Makefile @@ -133,6 +133,9 @@ px4fmu-v2_default: px4fmu-v4_default: $(call cmake-build,nuttx_px4fmu-v4_default) +px4-stm32f4discovery_default: + $(call cmake-build,nuttx_px4-stm32f4discovery_default) + px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake new file mode 100644 index 0000000000..8815992e40 --- /dev/null +++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake @@ -0,0 +1,87 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/led + drivers/boards/px4-stm32f4discovery + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/reboot + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/ver + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/conversion + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index ae3d95eaf5..783886e373 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -467,6 +467,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "px4-stm32f4discovery") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "aerocore") set(cpu_flags -mcpu=cortex-m4 diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c index fb1b282364..1c74be9058 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c @@ -129,18 +129,6 @@ stm32_boardinitialize(void) #include -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif - __EXPORT int nsh_archinitialize(void) { diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 895eec044a..2b24b477b5 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -51,7 +51,7 @@ if(NOT ${BOARD} STREQUAL "sim") ) if (config_io_board) add_dependencies(romfs fw_io) - endif() + endif() set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) From 5d6f63af1901536e1a41690c8ea95e2fe0c59975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:15:33 +0100 Subject: [PATCH 04/45] update jMAVSim --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 4267771753..eabdbee894 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 4267771753c4bb5d0696a678856620229523162e +Subproject commit eabdbee894be672e8913df619ed725b341a4e0cf From 5dc4ea8146d591e01897856a6cbd312130b137c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:17:59 +0100 Subject: [PATCH 05/45] Sim: minor cleanups --- src/modules/simulator/simulator_mavlink.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 507f220a43..2b993c31f9 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -426,6 +426,13 @@ void Simulator::initializeSensorData() void Simulator::pollForMAVLinkMessages(bool publish) { + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np("sim_rcv"); +#else + pthread_setname_np(pthread_self(), "sim_rcv"); +#endif + // udp socket data struct sockaddr_in _myaddr; const int _port = UDP_PORT; @@ -504,14 +511,13 @@ void Simulator::pollForMAVLinkMessages(bool publish) pret = ::poll(&fds[0], fd_count, 100); } - PX4_INFO("Found initial message, pret = %d", pret); _initialized = true; // reset system time (void)hrt_reset(); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); - PX4_INFO("Sending initial controls message to jMAVSim."); + PX4_INFO("Sending initial controls message to simulator"); send_controls(); } @@ -523,14 +529,6 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); - - // set the threads name -#ifdef __PX4_DARWIN - pthread_setname_np("sim_rcv"); -#else - pthread_setname_np(pthread_self(), "sim_rcv"); -#endif - // wait for new mavlink messages to arrive while (true) { From 947aa183f677bc62fc049445b8212f28d932334c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:22:06 +0100 Subject: [PATCH 06/45] Use 900 us as default motor off PWM since some ESCs have lower limits --- 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 06d1f10d47..5d9c6901a1 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -81,7 +81,7 @@ __BEGIN_DECLS /** * Default value for a shutdown motor */ -#define PWM_MOTOR_OFF 800 +#define PWM_MOTOR_OFF 900 /** * Default minimum PWM in us From fb3fade653ceefb3f682380fb8be444b363f7b74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:22:37 +0100 Subject: [PATCH 07/45] VTOL: Use correct motor off define --- src/modules/vtol_att_control/tiltrotor.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 17b3af75df..41da1d31c2 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -428,7 +428,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) break; case DISABLED: - pwm_value = PWM_LOWEST_MAX; + pwm_value = PWM_MOTOR_OFF; _rear_motors = DISABLED; break; @@ -446,21 +446,21 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) if (fd < 0) {PX4_WARN("can't open %s", dev);} ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); + struct pwm_output_values pwm_max_values; + memset(&pwm_max_values, 0, sizeof(pwm_max_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { - pwm_values.values[i] = pwm_value; + pwm_max_values.values[i] = pwm_value; } else { - pwm_values.values[i] = PWM_DEFAULT_MAX; + pwm_max_values.values[i] = PWM_DEFAULT_MAX; } - pwm_values.channel_count = _params->vtol_motor_count; + pwm_max_values.channel_count = _params->vtol_motor_count; } - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); if (ret != OK) {PX4_WARN("failed setting max values");} From 62763904f24db62ca8aafaac16495ebc48ac0cb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 17:12:29 +0100 Subject: [PATCH 08/45] Simulator: Add performance counter for incoming packet interval --- src/modules/simulator/simulator.h | 2 ++ src/modules/simulator/simulator_mavlink.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 182be7916b..5da68d002a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -230,6 +230,7 @@ private: _perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")), _perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")), _perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")), + _perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -270,6 +271,7 @@ private: perf_counter_t _perf_gps; perf_counter_t _perf_airspeed; perf_counter_t _perf_sim_delay; + perf_counter_t _perf_sim_interval; // uORB publisher handlers orb_advert_t _accel_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2b993c31f9..01899bf0ba 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -248,6 +248,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; perf_set(_perf_sim_delay, timestamp - sim_timestamp); + perf_count(_perf_sim_interval); if (publish) { publish_sensor_topics(&imu); From a4018bcbc1d554a2f06a1a0bc2a9872e40bf3f11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 17:13:22 +0100 Subject: [PATCH 09/45] Update to higher jMAVSim update rate --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index eabdbee894..0876a46d53 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit eabdbee894be672e8913df619ed725b341a4e0cf +Subproject commit 0876a46d53954db34775151f73db024bcfc656a0 From 4f548f328aa69f6c54336b34b9a86da9a18578ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 11:54:52 +0100 Subject: [PATCH 10/45] Set range to 0.9 Ga max since full scale range is 1.3 Ga --- src/modules/commander/mag_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 02b3a9f39a..ef0689bec2 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,7 +73,7 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take -static constexpr float MAG_MAX_OFFSET_LEN = 0.6f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions. +static constexpr float MAG_MAX_OFFSET_LEN = 0.9f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions. int32_t device_ids[max_mags]; int device_prio_max = 0; From 38fe768421924cdc149ae94baef451ec4a4be2f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:24:01 +0100 Subject: [PATCH 11/45] Start 2nd MAVLink instance in Gazebo SITL --- posix-configs/SITL/init/rcS_gazebo_iris | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index 95be984fb4..a85419a9e4 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -50,6 +50,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 From 85b3de0b0058b6b4639d501dda466b81de9a99d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:25:27 +0100 Subject: [PATCH 12/45] Start 2nd MAVLink instance in jMAVSim SITL --- posix-configs/SITL/init/rcS_jmavsim_iris | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index f7c4982ed5..6fd1d59db1 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -57,6 +57,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 From eff94677a47382146a3437cef49539eaee0bebb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:26:09 +0100 Subject: [PATCH 13/45] MAVLink: Only broadcast heartbeat on local network if not in onboard mode --- src/modules/mavlink/mavlink_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8e575e1b8f..c3004add29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -896,9 +896,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID struct telemetry_status_s &tstatus = get_rx_status(); /* resend heartbeat via broadcast */ - if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || + if ((_mode != MAVLINK_MODE_ONBOARD) + && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || (tstatus.heartbeat_time == 0)) && - msgid == MAVLINK_MSG_ID_HEARTBEAT) { + msgid == MAVLINK_MSG_ID_HEARTBEAT)) { int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); From d39e3137688c0f236bc9a482f166661f072750e9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 31 Dec 2015 11:50:52 +0100 Subject: [PATCH 14/45] use the proper check to prevent multiple mavlink instances on the same udp port, added warning when different remote than localhost connects to udp --- src/modules/mavlink/mavlink_main.cpp | 18 ++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 1 + 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c3004add29..10e0a3ac22 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1570,14 +1570,20 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(_device_name, this)) { - warnx("%s already running", _device_name); - return ERROR; - } - if (get_protocol() == SERIAL) { - warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + if (Mavlink::instance_exists(_device_name, this)) { + warnx("%s already running", _device_name); + return ERROR; + } + + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + } else if (get_protocol() == UDP) { + if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { + warnx("port %d already occupied", _network_port); + return ERROR; + } + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); } /* flush stdout in case MAVLink is about to take it over */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 51f08bb01c..e73d706c19 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1817,6 +1817,7 @@ MavlinkReceiver::receive_thread(void *arg) if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { // if we were sending to localhost before but have a new host then accept him memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); } #endif /* if read failed, this loop won't execute */ From 3d971e214ada0e2436b590f74dbd21db0ba756a8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 30 Nov 2015 21:44:28 +0100 Subject: [PATCH 15/45] don't update local position reference if home position changes --- .../position_estimator_inav_main.cpp | 45 ------------------- 1 file changed, 45 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 1da3ff305c..3244392829 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -273,7 +272,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); - hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -346,8 +344,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); - struct home_position_s home; - memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; @@ -373,7 +369,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); - int home_position_sub = orb_subscribe(ORB_ID(home_position)); int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); /* advertise */ @@ -677,46 +672,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - /* home position */ - orb_check(home_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(home_position), home_position_sub, &home); - - if (home.timestamp != home_timestamp) { - home_timestamp = home.timestamp; - - double est_lat, est_lon; - float est_alt; - - if (ref_inited) { - /* calculate current estimated position in global frame */ - est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); - } - - /* update reference */ - map_projection_init(&ref, home.lat, home.lon); - - /* update baro offset */ - baro_offset += home.alt - local_pos.ref_alt; - - local_pos.ref_lat = home.lat; - local_pos.ref_lon = home.lon; - local_pos.ref_alt = home.alt; - local_pos.ref_timestamp = home.timestamp; - - if (ref_inited) { - /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); - z_est[0] = -(est_alt - local_pos.ref_alt); - } - - ref_inited = true; - } - } - - /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ From bdaaca3d7889af2b05f77a40fef4a447f0231da3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 31 Dec 2015 14:40:56 +1100 Subject: [PATCH 16/45] make: Add missing ekf2 module to PX4-v1 build --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index d93e8b8b28..de8753341c 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -73,6 +73,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/ekf2 # # Vehicle Control From 28f5cb8fe6c3111398b7fe0114ca2a0f966eebc3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 31 Dec 2015 18:37:16 +1100 Subject: [PATCH 17/45] ekf2: Changes required to enter POSCTL mode Adds missing local position and global position data --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 121 ++++++++++++++++++++++++++++----- 2 files changed, 104 insertions(+), 19 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index eda69e727f..0cb210d045 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit eda69e727fab22c81c421b635cb34303b90b7968 +Subproject commit 0cb210d045d005d0775bc7b4c1631addb128c8e3 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 13f615c4e2..913b83227d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include @@ -111,6 +112,8 @@ public: void print(); + void print_status(); + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -123,6 +126,7 @@ private: orb_advert_t _att_pub; orb_advert_t _lpos_pub; orb_advert_t _control_state_pub; + orb_advert_t _vehicle_global_position_pub; /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -162,6 +166,11 @@ void Ekf2::print() _ekf->printStoredIMU(); } +void Ekf2::print_status() +{ + warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]"); +} + void Ekf2::task_main() { // subscribe to relevant topics @@ -244,12 +253,13 @@ void Ekf2::task_main() _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } - struct vehicle_attitude_s att; - struct vehicle_local_position_s lpos; - att.timestamp = hrt_absolute_time(); - lpos.timestamp = hrt_absolute_time(); + // run the EKF update _ekf->update(); + // generate vehicle attitude data + struct vehicle_attitude_s att; + att.timestamp = hrt_absolute_time(); + _ekf->copy_quaternion(att.q); matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); matrix::Euler euler(q); @@ -257,25 +267,65 @@ void Ekf2::task_main() att.pitch = euler(1); att.yaw = euler(2); + // generate vehicle local position data + struct vehicle_local_position_s lpos; float pos[3] = {}; float vel[3] = {}; + lpos.timestamp = hrt_absolute_time(); + + // Position in local NED frame _ekf->copy_position(pos); lpos.x = pos[0]; lpos.y = pos[1]; lpos.z = pos[2]; + // Velocity in NED frame (m/s) _ekf->copy_velocity(vel); lpos.vx = vel[0]; lpos.vy = vel[1]; lpos.vz = vel[2]; + // TODO: better status reporting + lpos.xy_valid = _ekf->position_is_valid(); + lpos.z_valid = true; + lpos.v_xy_valid = _ekf->position_is_valid(); + lpos.v_z_valid = true; + + // Position of local NED origin in GPS / WGS84 frame + lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set + lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) + lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) + lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees + lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + + // TODO: uORB definition does not define what this variable is. We have assumed it to be heading angle in radians lying on the range from +-pi + lpos.yaw = euler(2); + + // TODO: Distance to surface. Units not defined. + lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) + lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate + lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found + lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid + + // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres + // TODO: Should use sqrt of filter position variances + lpos.eph = gps.eph; + lpos.epv = gps.epv; + + // publish vehicle local position data + if (_lpos_pub == nullptr) { + _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); + } else { + orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + } + + // generate control state data control_state_s ctrl_state = {}; ctrl_state.timestamp = hrt_absolute_time(); ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); ctrl_state.q[0] = q(0); @@ -283,6 +333,14 @@ void Ekf2::task_main() ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); + // publish control state data + if (_control_state_pub == nullptr) { + _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + } else { + orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); + } + + // generate vehicle attitude data att.q[0] = q(0); att.q[1] = q(1); att.q[2] = q(2); @@ -293,25 +351,51 @@ void Ekf2::task_main() att.pitchspeed = sensors.gyro_rad_s[1]; att.yawspeed = sensors.gyro_rad_s[2]; - if (_control_state_pub == nullptr) { - _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - } else { - orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); - } - + // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } - if (_lpos_pub == nullptr) { - _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); - } else { - orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + // generate and publish global position data + struct vehicle_global_position_s global_pos; + if (_ekf->position_is_valid()) { + // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator + + global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start + global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds + + double est_lat, est_lon; + map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; // Latitude in degrees + global_pos.lon = est_lon; // Longitude in degrees + + global_pos.alt = -pos[2]; // Altitude AMSL in meters + + global_pos.vel_n = vel[0]; // Ground north velocity, m/s + global_pos.vel_e = vel[1]; // Ground east velocity, m/s + global_pos.vel_d = vel[2]; // Ground downside velocity, m/s + + global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. + + global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally + global_pos.epv = gps.epv; // Standard deviation of position vertically + + // TODO: implement terrain estimator + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + // TODO use innovatun consistency check timouts to set this + global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning + + global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) + + if (_vehicle_global_position_pub == nullptr) { + _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + } else { + orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); + } } - - } } @@ -394,6 +478,7 @@ int ekf2_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (ekf2::instance) { PX4_WARN("running"); + ekf2::instance->print_status(); return 0; } else { From 635d9ea760c2a6410a441085187de6124cca5cd2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 13:38:02 +1100 Subject: [PATCH 18/45] msg: Improve vehicle_local_position documentation Clean up formatting, improve consistency of descriptions and ensure units are defined --- msg/vehicle_local_position.msg | 50 +++++++++++++++++----------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 4da027ae77..6547f6defd 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,36 +1,36 @@ # Fused local position in NED. -uint64 timestamp # Time of this estimate, in microseconds since system start -bool xy_valid # true if x and y are valid -bool z_valid # true if z is valid -bool v_xy_valid # true if vy and vy are valid -bool v_z_valid # true if vz is valid +uint64 timestamp # Time of this estimate since system start, (microseconds) +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid # Position in local NED frame -float32 x # X position in meters in NED earth-fixed frame -float32 y # X position in meters in NED earth-fixed frame -float32 z # Z position in meters in NED earth-fixed frame (negative altitude) +float32 x # North position in NED earth-fixed frame, (metres) +float32 y # East position in NED earth-fixed frame, (metres) +float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres) # Velocity in NED frame -float32 vx # Ground X Speed (Latitude), m/s in NED -float32 vy # Ground Y Speed (Longitude), m/s in NED -float32 vz # Ground Z Speed (Altitude), m/s in NED +float32 vx # North velocity in NED earth-fixed frame, (metres/sec) +float32 vy # East velocity in NED earth-fixed frame, (metres/sec) +float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) # Heading -float32 yaw +float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) -# Reference position in GPS / WGS84 frame -bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) -bool z_global # true if z is valid and has valid global reference (ref_alt) -uint64 ref_timestamp # Time when reference position was set -float64 ref_lat # Reference point latitude in degrees -float64 ref_lon # Reference point longitude in degrees -float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level +# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set since system start, (microseconds) +float64 ref_lat # Reference point latitude, (degrees) +float64 ref_lon # Reference point longitude, (degrees) +float32 ref_alt # Reference altitude AMSL, MUST be set to current (not at reference point!) ground level, (metres) # Distance to surface -float32 dist_bottom # Distance to bottom surface (ground) -float32 dist_bottom_rate # Distance to bottom surface (ground) change rate -uint64 surface_bottom_timestamp # Time when new bottom surface found -bool dist_bottom_valid # true if distance to bottom surface is valid -float32 eph -float32 epv +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +float32 dist_bottom_rate # Rate of change of distance from bottom surface to ground, (metres/sec) +uint64 surface_bottom_timestamp # Time when new bottom surface found since system start, (microseconds) +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) \ No newline at end of file From 092b0d5dfba7932030250ee302b500c4b7900028 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 13:38:21 +1100 Subject: [PATCH 19/45] msg: Improve vehicle_global_position documentation Clean up formatting, improve consistency of descriptions and ensure units are defined --- msg/vehicle_global_position.msg | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index cee381e871..8aa7917df0 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -4,18 +4,18 @@ # 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. # -uint64 timestamp # Time of this estimate, in microseconds since system start -uint64 time_utc_usec # GPS UTC timestamp in microseconds -float64 lat # Latitude in degrees -float64 lon # Longitude in degrees -float32 alt # Altitude AMSL in meters -float32 vel_n # Ground north velocity, m/s -float32 vel_e # Ground east velocity, m/s -float32 vel_d # Ground downside velocity, m/s -float32 yaw # Yaw in radians -PI..+PI. -float32 eph # Standard deviation of position estimate horizontally -float32 epv # Standard deviation of position vertically -float32 terrain_alt # Terrain altitude in m, WGS84 +uint64 timestamp # Time of this estimate since system start, (microseconds) +uint64 time_utc_usec # GPS UTC timestamp, (microseconds) +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +float32 vel_n # North velocity in NED earth-fixed frame, (metres/sec) +float32 vel_e # East velocity in NED earth-fixed frame, (metres/sec) +float32 vel_d # Down velocity in NED earth-fixed frame, (metres/sec) +float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 terrain_alt # Terrain altitude WGS84, (metres) bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning -float32 pressure_alt # Pressure altitude +float32 pressure_alt # Pressure altitude AMSL, (metres) From 9264cec8070eefbfdf66d4af412b0ad74bc7dea5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 13:39:16 +1100 Subject: [PATCH 20/45] msg: Improve vehicle_gps_position documentation Clean up formatting, improve consistency of descriptions and ensure units are defined. --- msg/vehicle_gps_position.msg | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 718144f327..697bfb51d5 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -1,30 +1,30 @@ # GPS position in WGS84 coordinates. -uint64 timestamp_position # Timestamp for position information -int32 lat # Latitude in 1E-7 degrees +uint64 timestamp_position # Time of the position estimates since system start, (microseconds) +int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees -int32 alt # Altitude in 1E-3 meters (millimeters) above MSL -int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid +int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) +int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) -uint64 timestamp_variance -float32 s_variance_m_s # speed accuracy estimate m/s -float32 c_variance_rad # course accuracy estimate rad +uint64 timestamp_variance # Time of the accuracy estimates since system start, (microseconds) +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. -float32 eph # GPS HDOP horizontal dilution of position in m -float32 epv # GPS VDOP horizontal dilution of position in m +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) int32 noise_per_ms # GPS noise per millisecond int32 jamming_indicator # indicates jamming is occurring -uint64 timestamp_velocity # Timestamp for velocity informations -float32 vel_m_s # GPS ground speed (m/s) -float32 vel_n_m_s # GPS ground speed in m/s -float32 vel_e_m_s # GPS ground speed in m/s -float32 vel_d_m_s # GPS ground speed in m/s -float32 cog_rad # Course over ground (NOT heading, but direction of movement) in rad, -PI..PI -bool vel_ned_valid # Flag to indicate if NED speed is valid +uint64 timestamp_velocity # Time of the velocity estimates since system start, (microseconds) +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) +bool vel_ned_valid # True if NED velocity is valid -uint64 timestamp_time # Timestamp for time information +uint64 timestamp_time # Time of the UTC timestamp since system start, (microseconds) uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 uint8 satellites_used # Number of satellites used From 61ff954d26b75e4de949f479fce0c55d963019a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 11:38:56 +0100 Subject: [PATCH 21/45] uORB msg spec: Clarify local position yaw --- msg/vehicle_local_position.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 6547f6defd..89c141c417 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -17,7 +17,7 @@ float32 vy # East velocity in NED earth-fixed frame, (metres/sec) float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) # Heading -float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) From 276855acd297ce8abd3ff7af8875ef441b4a0e2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 11:39:11 +0100 Subject: [PATCH 22/45] EKF2: Remove todo --- src/modules/ekf2/ekf2_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 913b83227d..2bd5d25fa0 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -300,11 +300,10 @@ void Ekf2::task_main() lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level - // TODO: uORB definition does not define what this variable is. We have assumed it to be heading angle in radians lying on the range from +-pi - lpos.yaw = euler(2); + // The rotation of the tangent plane vs. geographical north + lpos.yaw = 0.0f; - // TODO: Distance to surface. Units not defined. - lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) + lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid From ce43c79a5ae9b039eb1f48a03ed403b67626015d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 11:42:25 +0100 Subject: [PATCH 23/45] Update ECL use --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 0cb210d045..7537c966f9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 0cb210d045d005d0775bc7b4c1631addb128c8e3 +Subproject commit 7537c966f95707f90d343441e4216de91334db5c From dc11b8a7efa8a18de2b2c8a68f70c6849775bcf5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 12:30:52 +0100 Subject: [PATCH 24/45] EKF2 sitl target: start normal app selection --- cmake/configs/posix_sitl_ekf2.cmake | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake index aefe3255d9..20c2d82ff9 100644 --- a/cmake/configs/posix_sitl_ekf2.cmake +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -32,18 +32,24 @@ set(config_module_list modules/sensors modules/simulator modules/mavlink - modules/dataman modules/attitude_estimator_ekf - #modules/attitude_estimator_q + modules/attitude_estimator_q modules/ekf_att_pos_estimator + modules/ekf2 modules/position_estimator_inav + modules/navigator + modules/vtol_att_control modules/mc_pos_control modules/mc_att_control - modules/navigator + modules/mc_pos_control_multiplatform + modules/mc_att_control_multiplatform + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 modules/commander - modules/vtol_att_control modules/controllib - modules/ekf2 lib/mathlib lib/mathlib/math/filter lib/conversion @@ -54,6 +60,8 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery + examples/px4_simple_app ) set(config_extra_builtin_cmds From e50fdbe32775edb6ee907eb76811da5879526423 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 12:31:56 +0100 Subject: [PATCH 25/45] Update ECL --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 7537c966f9..52cb9d02c7 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 7537c966f95707f90d343441e4216de91334db5c +Subproject commit 52cb9d02c7ed46c06d34ce7edb1d24d4cebdecba From 56e5c5070370ae4be07fd40ab37f89432b8ece12 Mon Sep 17 00:00:00 2001 From: DroneBuster Date: Tue, 29 Dec 2015 19:48:48 +0200 Subject: [PATCH 26/45] VTOL: Fix MOT_OFF bug --- src/modules/vtol_att_control/tiltrotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 41da1d31c2..c5e3ba7465 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -145,7 +145,7 @@ int Tiltrotor::get_motor_off_channels(int channels) break; } - channel_bitmap |= 1 << channel; + channel_bitmap |= 1 << (channel - 1); channels = channels / 10; } From 3b2e82cd62266555fd417c7de5331266f464b99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 14:44:42 +0100 Subject: [PATCH 27/45] VTOL: Fix motor index use in VT_FW_MOT_OFF. Create new param to re-default all deployed vehicles to not shut down motors. --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 2 +- src/modules/vtol_att_control/tiltrotor_params.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index d840f6c7e0..96569ddc94 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -46,6 +46,6 @@ set PWM_AUX_MAX 2000 set MAV_TYPE 21 param set VT_MOT_COUNT 6 -param set VT_FW_MOT_OFF 23 +param set VT_FW_MOT_OFFID 34 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 1 diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c5e3ba7465..81cb1cb710 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -67,7 +67,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF"); + _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID"); } Tiltrotor::~Tiltrotor() diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 58062c40b0..e37b520473 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); * * * @min 0 - * @max 123456 + * @max 12345678 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0); +PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); From 4ba7408ba0e47a92783086cc1bf4df5035c76235 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 21:21:49 +0100 Subject: [PATCH 28/45] Speed up Vagrant --- Vagrantfile | 29 +++++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index dfa4da4d8b..685d346e59 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -37,17 +37,42 @@ Vagrant.configure(2) do |config| # the path on the host to the actual folder. The second argument is # the path on the guest to mount the folder. And the optional third # argument is a set of non-required options. + # NFS should be faster: https://stefanwrobel.com/how-to-make-vagrant-performance-not-suck config.vm.synced_folder ".", "/Firmware", type: "nfs" # Provider-specific configuration so you can fine-tune various # backing providers for Vagrant. These expose provider-specific options. # Example for VirtualBox: # + + # This is to configure the machine to be as fast as possible + # Alternative: https://github.com/rdsubhas/vagrant-faster config.vm.provider "virtualbox" do |vb| # Display the VirtualBox GUI when booting the machine vb.gui = false vb.customize ["modifyvm", :id, "--ioapic", "on"] - vb.customize ["modifyvm", :id, "--cpus", "2"] + #vb.customize ["modifyvm", :id, "--cpus", "2"] + + config.vm.provider "virtualbox" do |v| + host = RbConfig::CONFIG['host_os'] + + # Give VM 1/4 system memory & access to all cpu cores on the host + if host =~ /darwin/ + cpus = `sysctl -n hw.ncpu`.to_i + # sysctl returns Bytes and we need to convert to MB + mem = `sysctl -n hw.memsize`.to_i / 1024 / 1024 / 4 + elsif host =~ /linux/ + cpus = `nproc`.to_i + # meminfo shows KB and we need to convert to MB + mem = `grep 'MemTotal' /proc/meminfo | sed -e 's/MemTotal://' -e 's/ kB//'`.to_i / 1024 / 4 + else # sorry Windows folks, I can't help you + cpus = 2 + mem = 1024 + end + + v.customize ["modifyvm", :id, "--memory", mem] + v.customize ["modifyvm", :id, "--cpus", cpus] + end # Since make and other tools freak out if they see timestamps # from the future and we share directories, tightly lock the host and guest clocks together (clock sync if more than 2 seconds off) @@ -57,7 +82,7 @@ Vagrant.configure(2) do |config| vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", "1"] # Customize the amount of memory on the VM: - vb.memory = "2048" + #vb.memory = "2048" end # # View the documentation for the provider you are using for more From bbe69fbe45f33f86e499e5ee6c5d49713ac0beb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 01:06:22 +0100 Subject: [PATCH 29/45] Fix FMUv4 USB PID --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 5e9c276494..90aaf6261f 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -669,7 +669,7 @@ CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" # CONFIG_USBMSC is not set From 55ce8c0c42a0e7ca00f95a7473848c2f6c8bb8e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 01:27:29 +0100 Subject: [PATCH 30/45] Fix QuRT build error --- src/drivers/device/vdev_posix.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index d99c7cfc74..43c55bcc53 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -245,11 +245,14 @@ extern "C" { const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; + + #ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); if (nret || thread_name[0] == 0) { PX4_WARN("failed getting thread name"); } + #endif PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); From 2d72c95edaa0ca2353942b128409d75bb94d1645 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 09:30:51 +0100 Subject: [PATCH 31/45] Add new posix_eagle_default and qurt_eagle_default targets --- Makefile | 6 ++ cmake/configs/qurt_eagle_default.cmake | 90 +++++++++++++++++++ .../qurt/px4_layer/commands_default.c | 55 ++++++++++++ 3 files changed, 151 insertions(+) create mode 100644 cmake/configs/qurt_eagle_default.cmake create mode 100644 src/platforms/qurt/px4_layer/commands_default.c diff --git a/Makefile b/Makefile index 77abd708a6..0bd5797512 100644 --- a/Makefile +++ b/Makefile @@ -166,6 +166,12 @@ qurt_eagle_release: posix_eagle_release: $(call cmake-build,$@) +qurt_eagle_default: + $(call cmake-build,$@) + +posix_eagle_default: + $(call cmake-build,$@) + posix: posix_sitl_default sitl_deprecation: diff --git a/cmake/configs/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake new file mode 100644 index 0000000000..6d0e886cb7 --- /dev/null +++ b/cmake/configs/qurt_eagle_default.cmake @@ -0,0 +1,90 @@ +include(qurt/px4_impl_qurt) + +if ("${HEXAGON_DRIVERS_ROOT}" STREQUAL "") + message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") +endif() + +if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") + message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") +endif() + +include_directories(${HEXAGON_DRIVERS_ROOT}/inc) + +# For Actual flight we need to link against the driver dynamic libraries +set(target_libraries + -L${HEXAGON_DRIVERS_ROOT}/libs +# The plan is to replace these with our drivers +# mpu9x50 +# uart_esc +# csr_gps +# rc_receiver + ) + + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + modules/sensors +# The plan is to replace these with our drivers +# $(EAGLE_DRIVERS_SRC)/mpu9x50 +# $(EAGLE_DRIVERS_SRC)/uart_esc +# $(EAGLE_DRIVERS_SRC)/rc_receiver +# $(EAGLE_DRIVERS_SRC)/csr_gps + + # + # System commands + # + systemcmds/param + + # + # Estimation modules (EKF/ SO3 / other filters) + # + #modules/attitude_estimator_ekf + modules/ekf_att_pos_estimator + modules/attitude_estimator_q + modules/position_estimator_inav + + # + # Vehicle Control + # + modules/mc_att_control + modules/mc_pos_control + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/commander + modules/controllib + + # + # Libraries + # + lib/mathlib + lib/mathlib/math/filter + lib/geo + lib/geo_lookup + lib/conversion + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + + # + # QuRT port + # + platforms/common + platforms/qurt/px4_layer + platforms/posix/work_queue + + # + # sources for muorb over fastrpc + # + modules/muorb/adsp + ) diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/qurt/px4_layer/commands_default.c new file mode 100644 index 0000000000..80022147f0 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 commands_default.c + * Commands to run for the "qurt_eagle_default" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "commander start\n" + + ; + + return commands; + +} From c91df50d7ead9fcbe4ff4852bcdb649a10326f4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 09:59:06 +0100 Subject: [PATCH 32/45] VDev: fix code style --- src/drivers/device/vdev_posix.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 43c55bcc53..220862d1ad 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -246,13 +246,14 @@ extern "C" { const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; - #ifndef __PX4_QURT +#ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); if (nret || thread_name[0] == 0) { PX4_WARN("failed getting thread name"); } - #endif + +#endif PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); From 735c823b5767a8cb2427afdbb6d357351d11952b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 03:13:49 -0800 Subject: [PATCH 33/45] Remove unmaintained NuttX config --- Makefile | 3 --- cmake/configs/nuttx_sim_simple.cmake | 21 --------------------- 2 files changed, 24 deletions(-) delete mode 100644 cmake/configs/nuttx_sim_simple.cmake diff --git a/Makefile b/Makefile index 0bd5797512..ab1b2681cb 100644 --- a/Makefile +++ b/Makefile @@ -142,9 +142,6 @@ px4fmu-v2_simple: px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) -nuttx_sim_simple: - $(call cmake-build,$@) - posix_sitl_default: $(call cmake-build,$@) diff --git a/cmake/configs/nuttx_sim_simple.cmake b/cmake/configs/nuttx_sim_simple.cmake deleted file mode 100644 index 068c6409f2..0000000000 --- a/cmake/configs/nuttx_sim_simple.cmake +++ /dev/null @@ -1,21 +0,0 @@ -include(nuttx/px4_impl_nuttx) - -message(WARNING "this is a work in progress and doesn't build yet") - -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) - -set(config_module_list - #platforms/nuttx - #platforms/nuttx/px4_layer - platforms/common - #drivers/led - drivers/device - #modules/systemlib - #modules/uORB - #examples/px4_simple_app - #lib/mathlib/math/filter - #lib/conversion - ) - -set(config_extra_builtin_cmds - ) From 6f1e53c810d19a30af4c5cab30df451e10b67e53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 03:14:06 -0800 Subject: [PATCH 34/45] Fix POSIX eagle config --- cmake/configs/posix_eagle_default.cmake | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index b78d393ddf..9de00fb694 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -35,9 +35,10 @@ set(config_module_list lib/mathlib lib/mathlib/math/filter + lib/conversion + lib/ecl lib/geo lib/geo_lookup - lib/conversion lib/terrain_estimation lib/runway_takeoff lib/tailsitter_recovery From 0ed13a911ebd849e6591fcdcb049787aa7fb802a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 03:51:26 -0800 Subject: [PATCH 35/45] Get QuRT drivers out of the way, as we are using our own --- cmake/configs/qurt_eagle_default.cmake | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cmake/configs/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake index 6d0e886cb7..e39a96fe3e 100644 --- a/cmake/configs/qurt_eagle_default.cmake +++ b/cmake/configs/qurt_eagle_default.cmake @@ -1,14 +1,14 @@ include(qurt/px4_impl_qurt) -if ("${HEXAGON_DRIVERS_ROOT}" STREQUAL "") - message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") -endif() +#if ("${HEXAGON_DRIVERS_ROOT}" #STREQUAL "") +# message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") +#endif() -if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") - message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") -endif() +#if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") +# message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") +#endif() -include_directories(${HEXAGON_DRIVERS_ROOT}/inc) +#include_directories(${HEXAGON_DRIVERS_ROOT}/inc) # For Actual flight we need to link against the driver dynamic libraries set(target_libraries From ba90ba14b0dcbcf4de425db4254662ce93e3e965 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 17:17:27 +0100 Subject: [PATCH 36/45] NuttX configs: added px4fmu-v2_ekf2 target for EKF2 development on Pixhawk --- Makefile | 4 +- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 - ...imple.cmake => nuttx_px4fmu-v2_ekf2.cmake} | 69 +++++++++++++++---- 3 files changed, 57 insertions(+), 17 deletions(-) rename cmake/configs/{nuttx_px4fmu-v2_simple.cmake => nuttx_px4fmu-v2_ekf2.cmake} (69%) diff --git a/Makefile b/Makefile index ab1b2681cb..2a5ab238f2 100644 --- a/Makefile +++ b/Makefile @@ -136,8 +136,8 @@ px4fmu-v4_default: px4-stm32f4discovery_default: $(call cmake-build,nuttx_px4-stm32f4discovery_default) -px4fmu-v2_simple: - $(call cmake-build,nuttx_px4fmu-v2_simple) +px4fmu-v2_ekf2: + $(call cmake-build,nuttx_px4fmu-v2_ekf2) px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index edf597406e..9b2c674167 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -83,7 +83,6 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav - modules/ekf2 # # Vehicle Control diff --git a/cmake/configs/nuttx_px4fmu-v2_simple.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake similarity index 69% rename from cmake/configs/nuttx_px4fmu-v2_simple.cmake rename to cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 070fbd703f..570082e97c 100644 --- a/cmake/configs/nuttx_px4fmu-v2_simple.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 1) + set(config_module_list # # Board support modules @@ -12,16 +14,14 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/px4io - drivers/boards/px4fmu-v2 + drivers/boards/px4fmu-v4 drivers/rgbled drivers/mpu6000 drivers/mpu9250 - drivers/lsm303d - drivers/l3gd20 drivers/hmc5883 drivers/ms5611 drivers/mb12xx + drivers/srf02 drivers/sf0x drivers/ll40ls drivers/trone @@ -42,6 +42,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands @@ -53,12 +54,40 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot + systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/tests + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + modules/position_estimator_inav + modules/ekf2 + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control # # Logging @@ -81,18 +110,32 @@ set(config_module_list #lib/mathlib/CMSIS lib/mathlib lib/mathlib/math/filter + lib/rc lib/ecl lib/external_lgpl lib/geo lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config - platforms/common + platforms/common platforms/nuttx/px4_layer + # + # OBC challenge + # + modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + # # Demo apps # @@ -103,18 +146,18 @@ set(config_module_list # Tutorial code from # https://px4.io/dev/daemon - examples/px4_daemon_app + #examples/px4_daemon_app # Tutorial code from # https://px4.io/dev/debug_values - examples/px4_mavlink_debug + #examples/px4_mavlink_debug # Tutorial code from # https://px4.io/dev/example_fixedwing_control - examples/fixedwing_control + #examples/fixedwing_control # Hardware test - examples/hwtest + #examples/hwtest ) set(config_extra_builtin_cmds @@ -122,16 +165,14 @@ set(config_extra_builtin_cmds sercon ) -set(config_io_board - px4io-v2 - ) - set(config_extra_libs ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver ) set(config_io_extra_libs - ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a ) add_custom_target(sercon) From 6e0f96c095762ec2cb7a9955c54fa874e63168c5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 3 Jan 2016 18:43:03 +1100 Subject: [PATCH 37/45] Update ECL --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 52cb9d02c7..b457709bd9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 52cb9d02c7ed46c06d34ce7edb1d24d4cebdecba +Subproject commit b457709bd9d7c215e4f245f217e5fadcb390fbdd From 2794ff2ddadb00c92d1be4d0e260b4e16bab2ec5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 15:27:45 +0100 Subject: [PATCH 38/45] Start shell only if SD card not present --- ROMFS/px4fmu_common/init.d/rcS | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a2a726bd7c..5e859ae222 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,9 +60,6 @@ unset FRC if [ $MODE == autostart ] then - # Try to get an USB console - # REBOOTWORK this needs to start after the flight control loop - nshterm /dev/ttyACM0 & # # Start the ORB (first app to start) @@ -844,6 +841,15 @@ then px4flow start & fi +# Start USB shell if no microSD present, MAVLink else +if [ $LOG_FILE == /dev/null ] +then + # Try to get an USB console + nshterm /dev/ttyACM0 & +else + mavlink start -r 800000 -d /dev/ttyACM0 -m config -x +fi + if [ $EXIT_ON_END == yes ] then echo "Exit from nsh" From a8a9c9b8ece94e8fedfd89dbdad2e94ac62f845c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 15:28:04 +0100 Subject: [PATCH 39/45] MAVLink: Start slightly differently on USB --- src/modules/mavlink/mavlink_main.cpp | 44 +++++++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 2 ++ 2 files changed, 45 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 10e0a3ac22..fc383fa996 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -661,9 +661,48 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -EINVAL; } + /* back off 1800 ms to avoid running into the USB setup timing */ + while (_mode == MAVLINK_MODE_CONFIG && + hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } + /* open uart */ _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + /* if this is a config link, stay here and wait for it to open */ + if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + + /* get the system arming state and abort on arming */ + while (_uart_fd < 0) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + orb_check(armed_fd, &updated); + + if (updated) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + return -1; + } + } + + usleep(100000); + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + }; + + close(armed_fd); + } + if (_uart_fd < 0) { return _uart_fd; } @@ -1595,9 +1634,12 @@ Mavlink::task_main(int argc, char *argv[]) /* default values for arguments */ _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original); - if (_uart_fd < 0) { + if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { warn("could not open %s", _device_name); return ERROR; + } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + /* the config link is optional */ + return OK; } #endif diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 37c66b5963..4f2967382c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -342,6 +342,8 @@ public: #endif static bool boot_complete() { return _boot_complete; } + bool is_usb_uart() { return _is_usb_uart; } + protected: Mavlink *next; From 77f869680aa60b79215afb4d32e7434dfb3b2f44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 17:16:47 +0100 Subject: [PATCH 40/45] Updated MAVLink protocol version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index db5cf66d1a..21d36c9e23 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit db5cf66d1a7f549ccbaa92b5476e78754e008297 +Subproject commit 21d36c9e23867b09145dc61b11941fb606726cc2 From ae2c46ac4c759ddcf8a57909f92a207d9d84521b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 19:03:17 +0100 Subject: [PATCH 41/45] Fix px4fmu-v2_ekf2 config --- cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 27 ++++++++++++------------ 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 570082e97c..0e456c738e 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -14,29 +14,32 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/boards/px4fmu-v4 + drivers/px4io + drivers/boards/px4fmu-v2 drivers/rgbled drivers/mpu6000 drivers/mpu9250 + drivers/lsm303d + drivers/l3gd20 drivers/hmc5883 drivers/ms5611 - drivers/mb12xx + #drivers/mb12xx drivers/srf02 drivers/sf0x drivers/ll40ls drivers/trone drivers/gps drivers/pwm_out_sim - drivers/hott - drivers/hott/hott_telemetry - drivers/hott/hott_sensors + #drivers/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors drivers/blinkm drivers/airspeed drivers/ets_airspeed drivers/meas_airspeed drivers/frsky_telemetry modules/sensors - drivers/mkblctrl + #drivers/mkblctrl drivers/px4flow drivers/oreoled drivers/gimbal @@ -54,14 +57,13 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot - systemcmds/topic_listener + #systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm systemcmds/mtd systemcmds/dumpfile systemcmds/ver - systemcmds/tests # # General system control @@ -76,7 +78,7 @@ set(config_module_list # # Estimation modules (EKF/ SO3 / other filters) # - modules/position_estimator_inav + # Too high RAM usage due to static allocations modules/ekf2 # @@ -110,7 +112,6 @@ set(config_module_list #lib/mathlib/CMSIS lib/mathlib lib/mathlib/math/filter - lib/rc lib/ecl lib/external_lgpl lib/geo @@ -123,13 +124,13 @@ set(config_module_list platforms/nuttx # had to add for cmake, not sure why wasn't in original config - platforms/common + platforms/common platforms/nuttx/px4_layer # # OBC challenge # - modules/bottle_drop + #modules/bottle_drop # # Rover apps @@ -142,7 +143,7 @@ set(config_module_list #examples/math_demo # Tutorial code from # https://px4.io/dev/px4_simple_app - examples/px4_simple_app + #examples/px4_simple_app # Tutorial code from # https://px4.io/dev/daemon From 0a5a5f4619ba0816cb128aec9243f9345a4f9577 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 22:44:19 +0100 Subject: [PATCH 42/45] Fix incomplete boot on new EKF config --- cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 0e456c738e..77526b4afd 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -2,7 +2,7 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) -set(config_uavcan_num_ifaces 1) +set(config_uavcan_num_ifaces 2) set(config_module_list # @@ -79,7 +79,10 @@ set(config_module_list # Estimation modules (EKF/ SO3 / other filters) # # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q modules/ekf2 + modules/position_estimator_inav # # Vehicle Control @@ -166,6 +169,10 @@ set(config_extra_builtin_cmds sercon ) +set(config_io_board + px4io-v2 + ) + set(config_extra_libs ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a uavcan From 3fb54e66b27efed56912fc71d6b6cfdaacb40a8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 23:57:06 +0100 Subject: [PATCH 43/45] ROMFS: Set 3DR quad tuning to more realistic default values --- ROMFS/px4fmu_common/init.d/10020_3dr_quad | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad index 26509d128d..a1b2e90270 100644 --- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad +++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad @@ -12,11 +12,11 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 6.5 - param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_P 0.14 param set MC_ROLLRATE_I 0.1 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 6.0 - param set MC_PITCHRATE_P 0.16 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.09 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 4 From 9415a6f8904268123ce686462135785e3eb9c8d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 15:10:22 +0100 Subject: [PATCH 44/45] Configure Easystar HIL setup to do Runway takeoff --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 2 ++ 1 file changed, 2 insertions(+) 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 7938c47bae..e37fc4389a 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,5 +39,7 @@ then param set FW_RR_P 0.3 fi +param set RWTO_TKOFF 1 + set HIL yes set MIXER AERT From 11ed5169cc3ce40fdd3389e938617394f3867eed Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 4 Jan 2016 23:53:34 +0100 Subject: [PATCH 45/45] don't use default source address for onboard udp link, wait on remote --- src/modules/mavlink/mavlink_main.cpp | 15 ++++++++++----- src/modules/mavlink/mavlink_main.h | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fc383fa996..a9c76322ba 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -930,7 +930,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID } #else if (get_protocol() == UDP) { - ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + if (get_client_source_initialized()) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } struct telemetry_status_s &tstatus = get_rx_status(); @@ -1045,11 +1047,14 @@ Mavlink::init_udp() return; } - /* set default target address */ + /* set default target address, but not for onboard mode (will be set on first recieved packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); - _src_addr.sin_family = AF_INET; - inet_aton("127.0.0.1", &_src_addr.sin_addr); - _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + if (_mode != MAVLINK_MODE_ONBOARD) { + set_client_source_initialized(); + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + } /* default broadcast address */ memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4f2967382c..1688398bb4 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -339,6 +339,10 @@ public: int get_socket_fd () { return _socket_fd; }; #ifdef __PX4_POSIX struct sockaddr_in * get_client_source_address() {return &_src_addr;}; + + void set_client_source_initialized() { _src_addr_initialized = true; }; + + bool get_client_source_initialized() { return _src_addr_initialized; }; #endif static bool boot_complete() { return _boot_complete; } @@ -423,6 +427,7 @@ private: struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; struct sockaddr_in _bcast_addr; + bool _src_addr_initialized; #endif int _socket_fd; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e73d706c19..231ea21c1d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,10 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg) struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); int localhost = (127 << 24) + 1; - if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) + || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); + _mavlink->set_client_source_initialized(); } #endif /* if read failed, this loop won't execute */