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..2a5ab238f2 100644 --- a/Makefile +++ b/Makefile @@ -133,15 +133,15 @@ px4fmu-v2_default: px4fmu-v4_default: $(call cmake-build,nuttx_px4fmu-v4_default) -px4fmu-v2_simple: - $(call cmake-build,nuttx_px4fmu-v2_simple) +px4-stm32f4discovery_default: + $(call cmake-build,nuttx_px4-stm32f4discovery_default) + +px4fmu-v2_ekf2: + $(call cmake-build,nuttx_px4fmu-v2_ekf2) px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) -nuttx_sim_simple: - $(call cmake-build,$@) - posix_sitl_default: $(call cmake-build,$@) @@ -163,6 +163,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/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 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 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/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" diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 4267771753..0876a46d53 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 4267771753c4bb5d0696a678856620229523162e +Subproject commit 0876a46d53954db34775151f73db024bcfc656a0 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 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/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 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 65% rename from cmake/configs/nuttx_px4fmu-v2_simple.cmake rename to cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 070fbd703f..77526b4afd 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 2) + set(config_module_list # # Board support modules @@ -21,27 +23,29 @@ set(config_module_list 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 drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands @@ -53,6 +57,7 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot + #systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm @@ -60,6 +65,35 @@ set(config_module_list systemcmds/dumpfile systemcmds/ver + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # 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 + # + # 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 # @@ -87,34 +121,47 @@ set(config_module_list 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/nuttx/px4_layer + # + # OBC challenge + # + #modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + # # Demo apps # #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 - 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 @@ -128,10 +175,12 @@ set(config_io_board 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) 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 - ) 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 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 diff --git a/cmake/configs/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake new file mode 100644 index 0000000000..e39a96fe3e --- /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/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/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 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) 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 diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 4da027ae77..89c141c417 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 transforming the tangent plane 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 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 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 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 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/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index d99c7cfc74..220862d1ad 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -245,12 +245,16 @@ 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); 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 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) diff --git a/src/lib/ecl b/src/lib/ecl index eda69e727f..b457709bd9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit eda69e727fab22c81c421b635cb34303b90b7968 +Subproject commit b457709bd9d7c215e4f245f217e5fadcb390fbdd 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; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 13f615c4e2..2bd5d25fa0 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,64 @@ 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 + + // The rotation of the tangent plane vs. geographical north + lpos.yaw = 0.0f; + + 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 + + // 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 +332,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 +350,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 +477,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 { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9d76fa8bf3..0620723efd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -662,9 +662,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; } @@ -892,14 +931,17 @@ 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(); /* 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)); @@ -1006,11 +1048,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)); @@ -1573,14 +1618,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 */ @@ -1592,9 +1643,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 c610bc1b74..530eb760b2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -340,9 +340,15 @@ 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; } + bool is_usb_uart() { return _is_usb_uart; } + protected: Mavlink *next; @@ -423,6 +429,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 51f08bb01c..231ea21c1d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,9 +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 */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 299b69b3d0..55628f3438 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(); } @@ -525,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); 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: 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 */ 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 507f220a43..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); @@ -426,6 +427,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 +512,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 +530,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) { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 17b3af75df..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() @@ -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; } @@ -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");} 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); 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; + +}