├── .gitignore ├── .gitmodules ├── .travis.yml ├── LICENSE ├── README.md ├── art ├── checkerboard.psd ├── compass_rose.svg ├── screenshot.png └── terrain.tgd ├── astylerc_java ├── build.xml ├── environment ├── HDR_040_Field_Bg.jpg ├── HDR_111_Parking_Lot_2_Bg.jpg ├── compass_rose.png ├── earth1.jpg ├── earth2.jpg ├── earth3.jpg ├── earth4.jpg ├── grass2.jpg ├── grass3.jpg ├── ground.jpg ├── ground2.jpg ├── sky_day.jpg └── sky_dusk.jpg ├── fix_style.sh ├── jMAVSim.iml ├── jMAVSim.ipr ├── jar-in-jar-loader.zip ├── lib ├── annotations.jar ├── asm4-all.jar ├── forms_rt.jar ├── gluegen-rt-natives-linux-amd64.jar ├── gluegen-rt-natives-linux-i586.jar ├── gluegen-rt-natives-macosx-universal.jar ├── gluegen-rt-natives-windows-amd64.jar ├── gluegen-rt-natives-windows-i586.jar ├── gluegen-rt.jar ├── j3dcore.jar ├── j3dutils.jar ├── javac2.jar ├── jdom.jar ├── joal-natives-linux-amd64.jar ├── joal-natives-linux-i586.jar ├── joal-natives-macosx-universal.jar ├── joal-natives-windows-amd64.jar ├── joal-natives-windows-i586.jar ├── joal.jar ├── jogl-all-natives-linux-amd64.jar ├── jogl-all-natives-linux-i586.jar ├── jogl-all-natives-macosx-universal.jar ├── jogl-all-natives-windows-amd64.jar ├── jogl-all-natives-windows-i586.jar ├── jogl-all.jar ├── jssc.jar └── vecmath.jar ├── mavlink └── message_definitions │ ├── common.xml │ └── minimal.xml ├── models ├── 3dr_arducopter_quad_x.mtl ├── 3dr_arducopter_quad_x.obj ├── cessna.mtl ├── cessna.obj ├── gimbal.png ├── x_vert.mtl └── x_vert.obj ├── src └── me │ └── drton │ └── jmavsim │ ├── CameraGimbal2D.java │ ├── DynamicObject.java │ ├── Environment.java │ ├── Filter.java │ ├── GNSSReport.java │ ├── KinematicObject.java │ ├── LogPlayerSensors.java │ ├── LogPlayerTarget.java │ ├── MAVLinkConnection.java │ ├── MAVLinkDisplayOnly.java │ ├── MAVLinkHILSystem.java │ ├── MAVLinkHILSystemBase.java │ ├── MAVLinkMissionItem.java │ ├── MAVLinkNode.java │ ├── MAVLinkPort.java │ ├── MAVLinkSystem.java │ ├── MavlinkTest.java │ ├── PeripherialBuzzer.java │ ├── ReportPanel.java │ ├── ReportUpdater.java │ ├── ReportUtil.java │ ├── ReportingObject.java │ ├── Rotor.java │ ├── SensorParamPanel.form │ ├── SensorParamPanel.java │ ├── Sensors.java │ ├── SerialMAVLinkPort.java │ ├── SerialPortChannel.java │ ├── SimpleEnvironment.java │ ├── SimpleSensors.java │ ├── SimpleTarget.java │ ├── Simulator.java │ ├── SystemOutHandler.java │ ├── TCPMavLinkPort.java │ ├── Target.java │ ├── UDPMavLinkPort.java │ ├── Visualizer3D.java │ ├── World.java │ ├── WorldObject.java │ └── vehicle │ ├── AbstractMulticopter.java │ ├── AbstractVehicle.java │ ├── Hexacopter.java │ └── Quadcopter.java └── tools └── RC2Serial └── RC2Serial.ino /.gitignore: -------------------------------------------------------------------------------- 1 | *.class 2 | out 3 | jMAVSim.iws 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "jMAVlib"] 2 | path = jMAVlib 3 | url = https://github.com/PX4/jMAVlib.git 4 | branch = master 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: false 2 | 3 | dist: trusty 4 | language: java 5 | 6 | jdk: 7 | - openjdk7 8 | - openjdk8 9 | - oraclejdk8 10 | 11 | before_script: 12 | - git submodule init 13 | - git submodule update --recursive 14 | 15 | script: 16 | - ant 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013, Anton Babushkin 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, this 11 | list of conditions and the following disclaimer in the documentation and/or 12 | other materials provided with the distribution. 13 | 14 | * Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 22 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 25 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/PX4/jMAVSim.svg?branch=master)](https://travis-ci.org/PX4/jMAVSim) 2 | 3 | Simple multirotor simulator with MAVLink protocol support 4 | 5 | ![screenshot](art/screenshot.png) 6 | 7 | ### Installation ### 8 | 9 | Requirements: 10 | * Java 8 or newer (JDK, http://www.oracle.com/technetwork/java/javase/downloads/index.html) 11 | 12 | * Java3D and JOGL/JOAL jars, including native libs for Linux (i586/64bit), Windows (i586/64bit) and Mac OS (universal) already included in this repository, no need to install it. 13 | 14 | * libvecmath-java (for ubuntu) 15 | 16 | Clone repository and initialize submodules: 17 | ``` 18 | git clone https://github.com/PX4/jMAVSim.git 19 | cd ~/jMAVSIM 20 | git submodule init 21 | git submodule update 22 | ``` 23 | 24 | Install prerequisites via HomeBrew: 25 | 26 | ``` 27 | brew install ant 28 | ``` 29 | 30 | Create a standalone runnable JAR file with all libraries included, copy supporting resources, and use a shorter command to execute: 31 | 32 | ``` 33 | ant create_run_jar copy_res 34 | cd out/production 35 | java -Djava.ext.dirs= -jar jmavsim_run.jar [any jMAVSim options] 36 | ``` 37 | 38 | To create a complete package ready for distribution, build the `distro` target (this will create `out/production/jMAVSim-distrib.zip`): 39 | 40 | ``` 41 | ant distro 42 | ``` 43 | 44 | To delete everything in the build folder `ant clean-all`. 45 | 46 | #### Alternate build / run / distribute 47 | 48 | Compile: 49 | ``` 50 | ant 51 | ``` 52 | 53 | Run: 54 | ``` 55 | java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator 56 | ``` 57 | 58 | Some shells (e.g. tcsh) will try to expand `*`, so use `\*` instead: 59 | ``` 60 | java -cp lib/\*:out/production/jmavsim.jar me.drton.jmavsim.Simulator 61 | ``` 62 | 63 | On **Windows** use `;` instead of `:` in -cp: 64 | ``` 65 | java -cp lib/*;out/production/jmavsim.jar me.drton.jmavsim.Simulator 66 | ``` 67 | 68 | #### Key command #### 69 | 70 | Views: 71 | - F - First-person-view camera. 72 | - S - Stationary ground camera. 73 | - G - Gimbal camera. 74 | - Z - Toggle auto-zoom for Stationary camera. 75 | - +/- - Zoom in/out 76 | - 0/ENTER - Reset zoom to default. 77 | 78 | Actions: 79 | - Q - Disable sim on MAV. 80 | - I - Enable sim on MAV. 81 | - H - Toggle HUD overlay. 82 | - C - Clear all messages on HUD. 83 | - R - Toggle data report sidebar. 84 | - T - Toggle data report updates. 85 | - D - Toggle sensor parameter control sidebar. 86 | - F1 - Show this key commands reference. 87 | - P - Pause. 88 | - ESC - Exit jMAVSim. 89 | - SPACE - Reset vehicle & view to start position. 90 | 91 | Manipulate Vehicle: 92 | - ARROW KEYS - Rotate around pitch/roll. 93 | - END/PG-DN - Rotate CCW/CW around yaw. 94 | - SHIFT + ARROWS - Move N/S/E/W. 95 | - SHIFT + INS/DEL - Move Up/Down. 96 | - NUMPAD 8/2/4/6 - Start/increase rotation rate around pitch/roll axis. 97 | - NUMPAD 1/3 - Start/increase rotation rate around yaw axis. 98 | - NUMPAD 5 - Stop all rotation. 99 | - CTRL + NUMPAD 5 - Reset vehicle attitude, velocity, & accelleration. 100 | 101 | Manipulate Environment: 102 | - ALT + 103 | - ARROW KEYS - Increase wind deviation in N/S/E/W direction. 104 | - INS/DEL - Increase wind deviation in Up/Down direction. 105 | - NUMPAD 8/2/4/6 - Increase wind speed in N/S/E/W direction. 106 | - NUMPAD 7/1 - Increase wind speed in Up/Down direction. 107 | - NUMPAD 5 - Stop all wind and deviations. 108 | 109 | - CTRL+ Manipulate - Rotate/move/increase at a higher/faster rate. 110 | 111 | ### Troubleshooting ### 112 | 113 | #### Java 3D 114 | 115 | jMAVSim uses java3d library for visualization. 116 | It was discontinued for long time, but now maintained again and uses JOGL backend. 117 | All necessary jars with java classes and native binaries (Linux/Mac OS/Windows) included in this repo, no need to install java3d manually. 118 | But need to make sure that java doesn't use any other deprecated version of java3d. 119 | For more info related to java3d see this article: https://gouessej.wordpress.com/2012/08/01/java-3d-est-de-retour-java-3d-is-back/ 120 | 121 | On **Mac OS** java may use deprecated version of java3d as extension, if you get following error: 122 | ``` 123 | JavaVM WARNING: JAWT_GetAWT must be called after loading a JVM 124 | AWT not found 125 | Exception in thread "main" java.lang.NoClassDefFoundError: apple/awt/CGraphicsDevice 126 | at javax.media.j3d.GraphicsConfigTemplate3D.(GraphicsConfigTemplate3D.java:55) 127 | ... 128 | ``` 129 | 130 | Then add `-Djava.ext.dirs=` option to command line when starting: 131 | ``` 132 | java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator 133 | ``` 134 | 135 | #### Serial port 136 | 137 | Serial port access is common problem. Make sure to hardcode correct port in Simulator.java: 138 | ``` 139 | serialMAVLinkPort.open("/dev/tty.usbmodem1", 230400, 8, 1, 0); 140 | ``` 141 | (Baudrate for USB ACM ports (that PX4 uses) has no effect, you can use any value) 142 | 143 | Usually port is: 144 | ``` 145 | Mac OS: /dev/tty.usbmodem1 146 | Linux: /dev/ttyACM0 147 | Windows: COM15 148 | ``` 149 | 150 | On **Linux** you may also get `Permission denied` error, add your user to `dialout` group and relogin, or just run as root. 151 | 152 | #### UDP 153 | 154 | UDP port used to connect ground station, e.g. qgroundcontrol. 155 | jMAVSim in this case works as bridge between ground station and autopilot (behavior can be configured of course). 156 | Make sure that jMAVSim and ground station use the same ports. 157 | In qgroundcontrol (or another GCS) you also need to add target host in UDP port configuration (localhost:14555), so both ends will know to which port they should send UDP packets. 158 | 159 | ### Development ### 160 | 161 | The simulator configuration is hardcoded in file `src/me/drton/jmavsim/Simulator.java`. Critical settings like port names or IP addresses can be provided as commandline arguments. 162 | 163 | New vehicle types (e.g. non standard multirotors configurations) can be added very easily. 164 | (But for fixed wing you will need some more aerodynamics knowledge). 165 | See files under `src/me/drton/jmavsim/vehicle/` as examples. 166 | 167 | The camera can be placed on any point, including gimabal, that can be controlled by autopilot, see `CameraGimbal2D` class and usage example (commented) in Simulator.java. 168 | 169 | Sensors data can be replayed from real flight log, use `LogPlayerSensors` calss for this. 170 | 171 | Custom vehicle visual models in .obj format can be used, edit this line: 172 | ``` 173 | AbstractMulticopter vehicle = new Quadcopter(world, "models/3dr_arducopter_quad_x.obj", "x", 0.33 / 2, 4.0, 0.05, 0.005, gc); 174 | ``` 175 | 176 | Custom MAVLink protocols can be used, no any recompilation needed, just specify XML file instead of `custom.xml` here: 177 | ``` 178 | MAVLinkSchema schema = new MAVLinkSchema("mavlink/message_definitions/common.xml"); 179 | ``` 180 | 181 | It's convenient to start the simulator from IDE. Free and powerful "IntelliJ IDEA" IDE recommended, project files for it are already included, just open project file `jMAVSim.ipr` and right-click -> Run `Simulator`. 182 | 183 | 184 | ### Exporting 3D vehicle models using Blender 185 | 186 | For custom vehicles it might be desirable to export new 3D models. One way of doing so is using [Blender](https://www.blender.org/). To import an existing model from another application into blender, (e.g. [onshape](https://cad.onshape.com)), export to the Collada (`.dae`) file format. 187 | 188 | Once the Collada file has been imported into blender, it is necessary to set the *Ambient* value in the shading property to `0.0` for all materials used. A value of `1.0` will produce white surfaces in jMAVSIM regardless of the material settings. 189 | 190 | When exporting from Blender, choose the `Wavefront (.obj)` file format. In the export dialogue, make the following changes: 191 | * Adjust the orientation of the model by specifying the `Forward` and `Up` directions. For example, if the z-axis in the Blender scene is pointing upwards, the correct setting for `Up:` would be `-Z up` for jMAVSIM. 192 | * Deselect "Objects as OBJ Objects" and select "Objects as OBJ Groups" instead. Otherwise jMAVSim will fail parsing the 3D model. 193 | -------------------------------------------------------------------------------- /art/checkerboard.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/art/checkerboard.psd -------------------------------------------------------------------------------- /art/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/art/screenshot.png -------------------------------------------------------------------------------- /astylerc_java: -------------------------------------------------------------------------------- 1 | indent=spaces=4 2 | style=java 3 | pad-oper 4 | pad-header 5 | unpad-paren 6 | lineend=linux 7 | add-braces 8 | indent-switches 9 | max-code-length=100 10 | keep-one-line-blocks 11 | keep-one-line-statements 12 | attach-namespaces 13 | max-continuation-indent=80 14 | max-code-length=100 15 | break-after-logical 16 | -------------------------------------------------------------------------------- /build.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 94 | 95 | 96 | 97 | 99 | 100 | 101 | 102 | 103 | 104 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | -------------------------------------------------------------------------------- /environment/HDR_040_Field_Bg.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/HDR_040_Field_Bg.jpg -------------------------------------------------------------------------------- /environment/HDR_111_Parking_Lot_2_Bg.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/HDR_111_Parking_Lot_2_Bg.jpg -------------------------------------------------------------------------------- /environment/compass_rose.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/compass_rose.png -------------------------------------------------------------------------------- /environment/earth1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/earth1.jpg -------------------------------------------------------------------------------- /environment/earth2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/earth2.jpg -------------------------------------------------------------------------------- /environment/earth3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/earth3.jpg -------------------------------------------------------------------------------- /environment/earth4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/earth4.jpg -------------------------------------------------------------------------------- /environment/grass2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/grass2.jpg -------------------------------------------------------------------------------- /environment/grass3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/grass3.jpg -------------------------------------------------------------------------------- /environment/ground.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/ground.jpg -------------------------------------------------------------------------------- /environment/ground2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/ground2.jpg -------------------------------------------------------------------------------- /environment/sky_day.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/sky_day.jpg -------------------------------------------------------------------------------- /environment/sky_dusk.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/environment/sky_dusk.jpg -------------------------------------------------------------------------------- /fix_style.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This script runs astyle and complains about lines that are too long 4 | # over all files ending in .h, .c, .cpp listed by git in the given 5 | # directory. 6 | # 7 | # Added by Julian Oes 8 | 9 | # Check for the latest astyle version 10 | ASTYLE_VER_REQUIRED_1="Artistic Style Version 2.05.1" 11 | ASTYLE_VER_REQUIRED_2="Artistic Style Version 2.06" 12 | ASTYLE_VER_REQUIRED_3="Artistic Style Version 3.0" 13 | ASTYLE_VER_REQUIRED_4="Artistic Style Version 3.0.1" 14 | ASTYLE_VER_REQUIRED_5="Artistic Style Version 3.1" 15 | 16 | astyle_ver() { 17 | echo "At least ${ASTYLE_VER_REQUIRED_1} is required" 18 | echo "You can get the correct version here: https://sourceforge.net/projects/astyle/files/astyle/astyle%202.06/" 19 | } 20 | 21 | # check if astyle is installed 22 | condition=$(which astyle 2>/dev/null | grep -v "not found" | wc -l) 23 | if [ $condition -eq 0 ]; then 24 | echo "astyle is not installed" 25 | astyle_ver 26 | exit 1 27 | else 28 | 29 | ASTYLE_VER=`astyle --version` 30 | 31 | if [ "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED_1" -a \ 32 | "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED_2" -a \ 33 | "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED_3" -a \ 34 | "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED_4" -a \ 35 | "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED_5" ]; 36 | then 37 | echo "Error: you're using ${ASTYLE_VER}" 38 | echo "but should be using ${ASTYLE_VER_REQUIRED} instead" 39 | exit 1 40 | fi 41 | fi 42 | 43 | # Check that exactly one directory is given 44 | if [ $# -eq 0 ]; then 45 | echo "No directory supplied" 46 | echo "Usage: ./fix_style.sh dir" 47 | exit 1 48 | 49 | elif [ $# -gt 1 ]; then 50 | echo "Too many directories supplied" 51 | echo "Usage: ./fix_style.sh dir" 52 | exit 1 53 | fi 54 | 55 | # Ignore files listed in style-ignore.txt 56 | style_ignore="style-ignore.txt" 57 | 58 | # Find the directory of this script because the file astylerc should be 59 | # next to it. 60 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 61 | 62 | # Keep track of errors for the exit value 63 | error_found=false 64 | 65 | # Use colordiff if available 66 | if command -v colordiff >/dev/null 2>&1; then 67 | diff_cmd=colordiff 68 | else 69 | diff_cmd=diff 70 | fi 71 | 72 | cd $1 > /dev/null 73 | 74 | # Go through all .h .cpp and .java files listed by git 75 | # TODO: add -r argument to include all files 76 | files=`git ls-files | grep -E "\.h$|\.cpp$|\.java$"` 77 | 78 | while IFS= read file; do 79 | 80 | # We want to check if the file is in the list to ignore. 81 | # We do this in a brute force way by looping through every 82 | # line the ignore file and compare it against the filename. 83 | if [[ -f $SCRIPT_DIR/$style_ignore ]]; then 84 | need_to_ignore=false 85 | while IFS= read ignore; do 86 | if [[ `echo $1/$file | grep "$ignore"` ]]; then 87 | need_to_ignore=true 88 | fi 89 | done < "$SCRIPT_DIR/$style_ignore" 90 | fi 91 | 92 | if [ "$need_to_ignore" = true ]; then 93 | # Don't do the astyle and file length checks, 94 | # go to next file. 95 | continue 96 | fi 97 | 98 | # Run astyle with given astylerc 99 | if [[ `echo $file | grep -E "\.java$"` ]]; then 100 | astylerc_file="astylerc_java" 101 | elif [[ `echo $file | grep -E "\.h$|\.cpp$"` ]]; then 102 | astylerc_file="astylerc_cpp" 103 | fi 104 | echo "Check $file using $astylerc_file" 105 | 106 | astyle_result=`astyle --options=$SCRIPT_DIR/$astylerc_file $file | grep "Formatted"` 107 | 108 | if [[ $astyle_result ]]; then 109 | echo "Formatted $file:" 110 | $diff_cmd $file $file.orig 111 | rm $file.orig 112 | error_found=true 113 | fi 114 | 115 | # TODO: this is disabled for now 116 | ## Go line by line 117 | #count=0 118 | #while IFS= read -r line; do 119 | # # Check for lines too long 120 | # len=${#line} 121 | # if [ $len -gt 100 ]; then 122 | # echo "Line $count too long" 123 | # error_found=true 124 | # fi 125 | # (( count++ )) 126 | #done < "$file" 127 | 128 | # We need to use this clunky way because otherwise 129 | # we lose the value of error_found. 130 | # See http://mywiki.wooledge.org/BashFAQ/024 131 | done < <(echo "$files") 132 | 133 | cd - > /dev/null 134 | 135 | if [ "$error_found" = true ]; then 136 | exit 1 137 | fi 138 | 139 | -------------------------------------------------------------------------------- /jMAVSim.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /jMAVSim.ipr: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | $PROJECT_DIR$/out/artifacts/jMAVSim_jar 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 194 | -------------------------------------------------------------------------------- /jar-in-jar-loader.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/jar-in-jar-loader.zip -------------------------------------------------------------------------------- /lib/annotations.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/annotations.jar -------------------------------------------------------------------------------- /lib/asm4-all.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/asm4-all.jar -------------------------------------------------------------------------------- /lib/forms_rt.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/forms_rt.jar -------------------------------------------------------------------------------- /lib/gluegen-rt-natives-linux-amd64.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/gluegen-rt-natives-linux-amd64.jar -------------------------------------------------------------------------------- /lib/gluegen-rt-natives-linux-i586.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/gluegen-rt-natives-linux-i586.jar -------------------------------------------------------------------------------- /lib/gluegen-rt-natives-macosx-universal.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/gluegen-rt-natives-macosx-universal.jar -------------------------------------------------------------------------------- /lib/gluegen-rt-natives-windows-amd64.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/gluegen-rt-natives-windows-amd64.jar -------------------------------------------------------------------------------- /lib/gluegen-rt-natives-windows-i586.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/gluegen-rt-natives-windows-i586.jar -------------------------------------------------------------------------------- /lib/gluegen-rt.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/gluegen-rt.jar -------------------------------------------------------------------------------- /lib/j3dcore.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/j3dcore.jar -------------------------------------------------------------------------------- /lib/j3dutils.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/j3dutils.jar -------------------------------------------------------------------------------- /lib/javac2.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/javac2.jar -------------------------------------------------------------------------------- /lib/jdom.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jdom.jar -------------------------------------------------------------------------------- /lib/joal-natives-linux-amd64.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/joal-natives-linux-amd64.jar -------------------------------------------------------------------------------- /lib/joal-natives-linux-i586.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/joal-natives-linux-i586.jar -------------------------------------------------------------------------------- /lib/joal-natives-macosx-universal.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/joal-natives-macosx-universal.jar -------------------------------------------------------------------------------- /lib/joal-natives-windows-amd64.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/joal-natives-windows-amd64.jar -------------------------------------------------------------------------------- /lib/joal-natives-windows-i586.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/joal-natives-windows-i586.jar -------------------------------------------------------------------------------- /lib/joal.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/joal.jar -------------------------------------------------------------------------------- /lib/jogl-all-natives-linux-amd64.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jogl-all-natives-linux-amd64.jar -------------------------------------------------------------------------------- /lib/jogl-all-natives-linux-i586.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jogl-all-natives-linux-i586.jar -------------------------------------------------------------------------------- /lib/jogl-all-natives-macosx-universal.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jogl-all-natives-macosx-universal.jar -------------------------------------------------------------------------------- /lib/jogl-all-natives-windows-amd64.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jogl-all-natives-windows-amd64.jar -------------------------------------------------------------------------------- /lib/jogl-all-natives-windows-i586.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jogl-all-natives-windows-i586.jar -------------------------------------------------------------------------------- /lib/jogl-all.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jogl-all.jar -------------------------------------------------------------------------------- /lib/jssc.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/jssc.jar -------------------------------------------------------------------------------- /lib/vecmath.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/lib/vecmath.jar -------------------------------------------------------------------------------- /models/3dr_arducopter_quad_x.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 4 3 | 4 | newmtl Blue_Metal 5 | Ns 129.411765 6 | Ka 0.000000 0.048686 0.647921 7 | Kd 0.000000 0.048686 0.647921 8 | Ks 1.000000 1.000000 1.000000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Gray_Plastic 15 | Ns 92.156863 16 | Ka 0.000000 0.000000 0.000000 17 | Kd 0.397538 0.397538 0.397538 18 | Ks 0.500000 0.500000 0.500000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Red_Metal 25 | Ns 129.411765 26 | Ka 0.673847 0.009976 0.042781 27 | Kd 0.673847 0.009976 0.042781 28 | Ks 1.000000 1.000000 1.000000 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Textolite 35 | Ns 92.156863 36 | Ka 0.000000 0.000000 0.000000 37 | Kd 0.008017 0.008017 0.008017 38 | Ks 0.500000 0.500000 0.500000 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | -------------------------------------------------------------------------------- /models/cessna.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'cessna.blend' 2 | # Material Count: 1 3 | 4 | newmtl test 5 | Ns 96.078431 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.640000 0.640000 0.640000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /models/gimbal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/jMAVSim/66b764ada522893c05224950aa6268c809f8e48a/models/gimbal.png -------------------------------------------------------------------------------- /models/x_vert.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'tailsitter.blend' 2 | # Material Count: 2 3 | 4 | # newmtl material_black 5 | # Ns 96.078431 6 | # Ka 0.000000 0.000000 0.000000 7 | # Kd 0.053447 0.053447 0.053447 8 | # Ks 0.500000 0.500000 0.500000 9 | # Ke 0.000000 0.000000 0.000000 10 | # Ni 1.000000 11 | # d 1.000000 12 | # illum 2 13 | 14 | newmtl material_white 15 | Ns 96.078431 16 | Ka 0.000000 0.000000 0.000000 17 | Kd 0.640000 0.640000 0.640000 18 | Ks 0.500000 0.500000 0.500000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/CameraGimbal2D.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavsim.vehicle.AbstractVehicle; 4 | 5 | import javax.media.j3d.Appearance; 6 | import javax.media.j3d.Material; 7 | import javax.media.j3d.Texture; 8 | import javax.media.j3d.TextureAttributes; 9 | import javax.media.j3d.Transform3D; 10 | import javax.media.j3d.TransformGroup; 11 | import javax.vecmath.Color3f; 12 | import javax.vecmath.Matrix3d; 13 | import javax.vecmath.Vector3d; 14 | 15 | import com.sun.j3d.utils.geometry.Sphere; 16 | import com.sun.j3d.utils.image.TextureLoader; 17 | 18 | import java.util.List; 19 | 20 | /** 21 | * User: ton Date: 21.03.14 Time: 23:22 22 | */ 23 | public class CameraGimbal2D extends KinematicObject implements ReportingObject { 24 | private DynamicObject baseObject; 25 | private int pitchChannel = -1; 26 | private double pitchScale = 1.0; 27 | private int rollChannel = -1; 28 | private double rollScale = 1.0; 29 | private Vector3d positionOffset = new Vector3d(0, 0, -0.045); // from base object 30 | private double rotOffset = Math.PI / 2.0; 31 | private float gimbalDia = 0.03f; 32 | private double[] controls = {0, 0}; 33 | private Sphere model; 34 | private TransformGroup gimbalTG; 35 | private Matrix3d rotM3d = new Matrix3d(); // temp storage 36 | 37 | public CameraGimbal2D(World world, String modelName, boolean showGui) { 38 | super(world, showGui); 39 | if (!modelName.isEmpty()) { 40 | 41 | Texture texture = new TextureLoader(modelName, null).getTexture(); 42 | texture.setBoundaryModeS(Texture.WRAP); 43 | texture.setBoundaryModeT(Texture.WRAP); 44 | 45 | Appearance app = new Appearance(); 46 | Color3f black = new Color3f(0.0f, 0.0f, 0.0f); 47 | Color3f white = new Color3f(1.0f, 1.0f, 1.0f); 48 | app.setMaterial(new Material(white, black, white, black, 10.0f)); 49 | 50 | TextureAttributes texAttr = new TextureAttributes(); 51 | texAttr.setTextureMode(TextureAttributes.REPLACE); 52 | app.setTexture(texture); 53 | app.setTextureAttributes(texAttr); 54 | 55 | // the actual model 56 | model = new Sphere(gimbalDia, Sphere.GENERATE_NORMALS | Sphere.GENERATE_TEXTURE_COORDS, 32, app); 57 | 58 | gimbalTG = new TransformGroup(); 59 | gimbalTG.setCapability(TransformGroup.ALLOW_TRANSFORM_READ); 60 | gimbalTG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE); 61 | 62 | Transform3D rotT3d = new Transform3D(); 63 | rotT3d.rotZ(rotOffset); 64 | gimbalTG.setTransform(rotT3d); 65 | 66 | gimbalTG.addChild(model); 67 | transformGroup.addChild(gimbalTG); 68 | } 69 | } 70 | 71 | public void report(StringBuilder builder) { 72 | builder.append("GIMBAL"); 73 | builder.append(newLine); 74 | builder.append("==========="); 75 | builder.append(newLine); 76 | 77 | builder.append("Att: "); 78 | builder.append(ReportUtil.vector2str(ReportUtil.vectRad2Deg(attitude))); 79 | builder.append(newLine); 80 | 81 | builder.append(String.format("Roll ctrl: %s", ReportUtil.d2str(controls[1]))); 82 | builder.append(newLine); 83 | builder.append(String.format("Pitch ctrl: %s", ReportUtil.d2str(controls[0]))); 84 | builder.append(newLine); 85 | } 86 | 87 | public void setBaseObject(DynamicObject object) { 88 | this.baseObject = object; 89 | } 90 | 91 | public Vector3d getPositionOffset() { 92 | return positionOffset; 93 | } 94 | 95 | public void setPositionOffset(Vector3d positionOffset) { 96 | this.positionOffset = positionOffset; 97 | } 98 | 99 | public void setPitchChannel(int pitchChannel) { 100 | this.pitchChannel = pitchChannel; 101 | } 102 | 103 | public void setPitchScale(double pitchScale) { 104 | this.pitchScale = pitchScale / 2; 105 | } 106 | 107 | public void setRollChannel(int channel) { 108 | this.rollChannel = channel; 109 | } 110 | 111 | public void setRollScale(double scale) { 112 | this.rollScale = scale / 2; 113 | } 114 | 115 | @Override 116 | public void update(long t, boolean paused) { 117 | this.position = (Vector3d) baseObject.position.clone(); 118 | this.attitude = (Vector3d) baseObject.attitude.clone(); 119 | this.rotation.rotZ(this.attitude.z); 120 | if ((pitchChannel >= 0 || rollChannel >= 0) && baseObject instanceof AbstractVehicle && 121 | ((AbstractVehicle) baseObject).getControl().size() > 0) { 122 | // Control camera pitch/roll 123 | List control = ((AbstractVehicle) baseObject).getControl(); 124 | 125 | if (rollChannel >= 0) { 126 | if (control.size() > rollChannel) { 127 | this.controls[0] = control.get(rollChannel); 128 | } 129 | this.attitude.x = (this.controls[0] * rollScale); 130 | } 131 | if (pitchChannel >= 0) { 132 | if (control.size() > pitchChannel) { 133 | this.controls[1] = control.get(pitchChannel); 134 | } 135 | this.attitude.y = (this.controls[1] * pitchScale); 136 | } 137 | } 138 | 139 | rotM3d.rotX(this.attitude.y); 140 | rotation.mul(rotM3d); 141 | rotM3d.rotY(this.attitude.x); 142 | rotation.mul(rotM3d); 143 | rotation.normalize(); 144 | 145 | Vector3d newOffset = (Vector3d) positionOffset.clone(); 146 | rotation.transform(newOffset); 147 | this.position.add(newOffset); 148 | 149 | } 150 | } 151 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/DynamicObject.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import javax.vecmath.AxisAngle4d; 4 | import javax.vecmath.Matrix3d; 5 | import javax.vecmath.Vector3d; 6 | 7 | /** 8 | * Abstract dynamic object class. 9 | * Calculates all kinematic parameters (attitude, attitude rates, position, velocity, acceleration) from force and torque acting on the vehicle. 10 | */ 11 | public abstract class DynamicObject extends KinematicObject { 12 | protected long lastTime = -1; 13 | protected double mass = 1.0; 14 | protected Matrix3d momentOfInertia = new Matrix3d(); 15 | protected Matrix3d momentOfInertiaInv = new Matrix3d(); 16 | 17 | // temp storage objects for calculations 18 | private Vector3d tmpVec = new Vector3d(); 19 | private Vector3d angularAcc = new Vector3d(); 20 | private Matrix3d rotMtx = new Matrix3d(); 21 | private AxisAngle4d rotAng = new AxisAngle4d(); 22 | 23 | public DynamicObject(World world, boolean showGui) { 24 | super(world, showGui); 25 | rotation.rotX(0); 26 | momentOfInertia.rotZ(0.0); 27 | momentOfInertiaInv.rotZ(0.0); 28 | } 29 | 30 | public double getMass() { 31 | return mass; 32 | } 33 | 34 | public void setMass(double mass) { 35 | this.mass = mass; 36 | } 37 | 38 | public void setMomentOfInertia(Matrix3d momentOfInertia) { 39 | this.momentOfInertia.set(momentOfInertia); 40 | this.momentOfInertiaInv.invert(momentOfInertia); 41 | } 42 | 43 | @Override 44 | public void update(long t, boolean paused) { 45 | if (paused) { 46 | return; 47 | } 48 | if (lastTime >= 0) { 49 | double dt = Math.max((t - lastTime) / 1000.0, 0.001); // constrain time step 50 | double grnd = getWorld().getEnvironment().getGroundLevelAt(position); 51 | 52 | // Position 53 | tmpVec.set(velocity); 54 | tmpVec.scale(dt); 55 | position.add(tmpVec); 56 | // Velocity 57 | acceleration = getForce(); 58 | acceleration.scale(1.0 / mass); 59 | if (!ignoreGravity) { 60 | acceleration.add(getWorld().getEnvironment().getG()); 61 | } 62 | if (position.z >= grnd && velocity.z + acceleration.z * dt >= 0.0) { 63 | // On ground 64 | // acceleration.x = -velocity.x / dt; 65 | // acceleration.y = -velocity.y / dt; 66 | // acceleration.z = -velocity.z / dt; 67 | position.z = grnd; 68 | acceleration.set(0.0, 0.0, 0.0); 69 | velocity.set(0.0, 0.0, 0.0); 70 | rotationRate.set(0.0, 0.0, 0.0); 71 | } else { 72 | tmpVec.set(acceleration); 73 | tmpVec.scale(dt); 74 | velocity.add(tmpVec); 75 | // Rotation 76 | if (rotationRate.length() > 0.0) { 77 | tmpVec.set(rotationRate); 78 | tmpVec.normalize(); 79 | rotAng.set(tmpVec, rotationRate.length() * dt); 80 | rotMtx.set(rotAng); 81 | rotation.mulNormalize(rotMtx); 82 | } 83 | // Rotation rate 84 | tmpVec.set(rotationRate); 85 | momentOfInertia.transform(tmpVec); 86 | angularAcc.cross(rotationRate, tmpVec); 87 | angularAcc.negate(); 88 | angularAcc.add(getTorque()); 89 | momentOfInertiaInv.transform(angularAcc); 90 | angularAcc.scale(dt); 91 | rotationRate.add(angularAcc); 92 | } 93 | attitude.set(utilMatrixToEulers(rotation)); 94 | } 95 | lastTime = t; 96 | } 97 | 98 | protected abstract Vector3d getForce(); 99 | protected abstract Vector3d getTorque(); 100 | protected abstract Vector3d getAirFlowForce(Vector3d airSpeed); 101 | } 102 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/Environment.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import javax.vecmath.Matrix3d; 4 | import javax.vecmath.Vector3d; 5 | 6 | /** 7 | * User: ton Date: 28.11.13 Time: 20:35 8 | */ 9 | public abstract class Environment extends WorldObject implements ReportingObject { 10 | protected Vector3d g = new Vector3d(); // gravity 11 | protected Vector3d magField = new Vector3d(); 12 | protected Vector3d wind = new Vector3d(); // base wind speed 13 | protected Vector3d windDeviation = new Vector3d(); // deviation magnitude 14 | protected Vector3d windCurrent = new Vector3d(); // current wind conditions (base plus deviation) 15 | protected double windT = 2.0; 16 | protected double groundLevel = 0.0; 17 | protected Float magIncl = 0.0f; 18 | protected Float magDecl = 0.0f; 19 | protected double magHIntensity; 20 | protected double magTIntensity; 21 | 22 | 23 | public Environment(World world) { 24 | super(world); 25 | } 26 | 27 | public void report(StringBuilder builder) { 28 | builder.append("ENVIRONMENT"); 29 | builder.append(newLine); 30 | builder.append("==========="); 31 | builder.append(newLine); 32 | 33 | builder.append("Grav: "); 34 | builder.append(ReportUtil.vector2str(g)); 35 | builder.append(newLine); 36 | 37 | builder.append("Mag: "); 38 | builder.append(ReportUtil.vector2str(magField)); 39 | builder.append(newLine); 40 | builder.append(String.format(" Incl: %.5f; Decl: %.5f", magIncl, magDecl)); 41 | builder.append(newLine); 42 | builder.append(String.format(" H-Magn: %.5f; T-Magn: %.5f", magHIntensity, magTIntensity)); 43 | builder.append(newLine); 44 | 45 | builder.append("Wind Set: "); 46 | builder.append(ReportUtil.vector2str(wind)); 47 | builder.append(newLine); 48 | builder.append("Wind Dev: "); 49 | builder.append(ReportUtil.vector2str(windDeviation)); 50 | builder.append(newLine); 51 | builder.append("Wind Cur: "); 52 | builder.append(ReportUtil.vector2str(windCurrent)); 53 | builder.append(newLine); 54 | 55 | builder.append(newLine); 56 | } 57 | 58 | 59 | public Vector3d getWind() { 60 | return wind; 61 | } 62 | public void setWind(Vector3d wind) { 63 | this.wind = wind; 64 | } 65 | 66 | public Vector3d getWindDeviation() { 67 | return this.windDeviation; 68 | } 69 | public void setWindDeviation(Vector3d deviation) { 70 | this.windDeviation = deviation; 71 | } 72 | 73 | /** 74 | * Get wind (air velocity) vector in specified point. 75 | * 76 | * @param point point in NED frame 77 | * @return wind vector 78 | */ 79 | public Vector3d getCurrentWind(Vector3d point) { 80 | return windCurrent; 81 | } 82 | public void setCurrentWind(Vector3d wind) { 83 | this.windCurrent = wind; 84 | } 85 | 86 | /** 87 | * The base ground level member. 88 | */ 89 | public void setGroundLevel(double groundLevel) { 90 | this.groundLevel = groundLevel; 91 | } 92 | public double getGroundLevel() { 93 | return groundLevel; 94 | } 95 | 96 | /** 97 | * Get ground level for specified point. 98 | * Multilevel environment may be simulated, in this case method should return level under specified point. 99 | * 100 | * @param point point in NED frame 101 | * @return ground level in NED frame 102 | */ 103 | public double getGroundLevelAt(Vector3d point) { 104 | return getGroundLevel(); 105 | } 106 | 107 | /** 108 | * Get gravity vector. 109 | * Should be pointed to ground. 110 | * 111 | * @return gravity vector 112 | */ 113 | public Vector3d getG() { 114 | return g; 115 | } 116 | public void setG(Vector3d grav) { 117 | if (grav == null) { 118 | g = new Vector3d(); 119 | } else { 120 | g = grav; 121 | } 122 | } 123 | 124 | /** 125 | * Get magnetic field vector in specified point. 126 | * 127 | * @param point point in NED frame 128 | * @return magnetic field vector 129 | */ 130 | public Vector3d getMagField(Vector3d point) { 131 | return magField; 132 | } 133 | 134 | /** 135 | * Set magnetic field using specific vector. 136 | * 137 | * @param magField vector in NED frame 138 | */ 139 | public void setMagField(Vector3d magField) { 140 | this.magField = magField; 141 | magHIntensity = Math.sqrt(magField.x * magField.x + magField.y * magField.y); 142 | magTIntensity = Math.sqrt(magHIntensity * magHIntensity + magField.z * magField.z); 143 | magIncl = (float)Math.toDegrees(Math.atan2(magField.z, magField.x)); 144 | magDecl = (float)Math.toDegrees(Math.atan2(magField.y, magField.x)); 145 | } 146 | 147 | /** 148 | * Set magnetic field using specified inclination and declination 149 | * 150 | * @param incl Magnetic inclination in degrees 151 | * @param decl Magnetic declination in degrees 152 | */ 153 | public void setMagFieldByInclDecl(double incl, double decl) { 154 | decl = Math.toRadians(decl); 155 | incl = Math.toRadians(incl); 156 | Vector3d magField = new Vector3d(Math.cos(incl), 0.0f, Math.sin(incl)); 157 | Matrix3d declMtx = new Matrix3d(); 158 | declMtx.rotZ(decl); 159 | declMtx.transform(magField); 160 | setMagField(magField); 161 | } 162 | 163 | } 164 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/Filter.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | public final class Filter { 4 | private double tc = 0.0; 5 | private double z1 = 0.0; 6 | 7 | /** 8 | * First-order fixed-timestep filter. 9 | * larger tau, smoother filter 10 | * 11 | */ 12 | public void filterInit(double dt, double tau, double setpoint) { 13 | this.tc = dt / tau; 14 | filterReset(setpoint); 15 | } 16 | 17 | public void filterReset(double setpoint) { 18 | this.z1 = setpoint; 19 | } 20 | 21 | double filter(double signal) { 22 | this.z1 += (signal - this.z1) * this.tc; 23 | return this.z1; 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/GNSSReport.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.geo.LatLonAlt; 4 | 5 | import javax.vecmath.Vector3d; 6 | 7 | /** 8 | * GNSS Report. 9 | */ 10 | public class GNSSReport { 11 | public LatLonAlt position; 12 | public float eph; 13 | public float epv; 14 | public Vector3d velocity; 15 | public int fix; // 0 = no fix, 1 = time only, 2 = 2D fix, 3 = 3D fix 16 | public long time; // UTC time in [us] 17 | 18 | /** 19 | * Get scalar horizontal speed. 20 | * 21 | * @return horizontal ground speed in [m/s] 22 | */ 23 | public double getSpeed() { 24 | return Math.sqrt(velocity.x * velocity.x + velocity.y * velocity.y); 25 | } 26 | 27 | /** 28 | * Get horizontal course over ground. 29 | * 30 | * @return horizontal course over ground in [rad] 31 | */ 32 | public double getCog() { 33 | return Math.atan2(velocity.y, velocity.x); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/KinematicObject.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import com.sun.j3d.loaders.IncorrectFormatException; 4 | import com.sun.j3d.loaders.ParsingErrorException; 5 | import com.sun.j3d.loaders.Scene; 6 | import com.sun.j3d.loaders.objectfile.ObjectFile; 7 | //import com.sun.j3d.utils.behaviors.mouse.MouseRotate; 8 | 9 | //import javax.media.j3d.BoundingSphere; 10 | import javax.media.j3d.BranchGroup; 11 | //import javax.media.j3d.Shape3D; 12 | import javax.media.j3d.Transform3D; 13 | import javax.media.j3d.TransformGroup; 14 | import javax.vecmath.Matrix3d; 15 | import javax.vecmath.Vector3d; 16 | 17 | import java.io.IOException; 18 | import java.net.URL; 19 | 20 | /** 21 | * Abstract kinematic object class. 22 | * Stores all kinematic parameters (attitude, attitude rates, position, velocity, acceleration) but doesn't calculate it. 23 | * These parameters may be set directly for objects moving by fixed trajectory or simulated from external forces (see DynamicObject). 24 | */ 25 | public abstract class KinematicObject extends WorldObject { 26 | protected boolean ignoreGravity = false; 27 | protected boolean ignoreWind = false; 28 | protected Vector3d position = new Vector3d(); 29 | protected Vector3d velocity = new Vector3d(); 30 | protected Vector3d acceleration = new Vector3d(); 31 | protected Matrix3d rotation = new Matrix3d(); 32 | protected Vector3d rotationRate = new Vector3d(); 33 | protected Vector3d attitude = new Vector3d(); 34 | 35 | protected Transform3D transform; 36 | protected TransformGroup transformGroup; 37 | protected BranchGroup branchGroup; 38 | 39 | public KinematicObject(World world, boolean showGui) { 40 | super(world); 41 | rotation.setIdentity(); 42 | if (showGui) { 43 | transformGroup = new TransformGroup(); 44 | transformGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_READ); 45 | transformGroup.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE); 46 | transform = new Transform3D(); 47 | transformGroup.setTransform(transform); 48 | branchGroup = new BranchGroup(); 49 | branchGroup.addChild(transformGroup); 50 | } 51 | } 52 | 53 | /** 54 | * Helper method to create model from .obj file. 55 | * 56 | * @param modelFile file name 57 | * @throws java.io.FileNotFoundException 58 | */ 59 | protected void modelFromFile(String modelFile) { 60 | URL file = null; 61 | try { 62 | file = new URL("file:./" + modelFile); 63 | 64 | ObjectFile objectFile = new ObjectFile(); 65 | Scene scene = objectFile.load(file); 66 | 67 | // Shape3D shape = (Shape3D)bg.getChild(0); 68 | // shape.setPickable(true); 69 | // TransformGroup objRotate = new TransformGroup(); 70 | // objRotate.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE); 71 | // objRotate.addChild(bg); 72 | // MouseRotate f1=new MouseRotate(); 73 | // f1.setSchedulingBounds(new BoundingSphere()); 74 | // f1.setTransformGroup(objRotate); 75 | // bg.addChild(f1); 76 | // transformGroup.addChild(objRotate); 77 | 78 | BranchGroup bg = scene.getSceneGroup(); 79 | transformGroup.addChild(bg); 80 | 81 | } catch (IOException | IncorrectFormatException | ParsingErrorException e) { 82 | System.out.println("ERROR: could not load 3D model: " + modelFile); 83 | System.out.println("Error message:" + e.getLocalizedMessage()); 84 | } 85 | 86 | } 87 | 88 | public BranchGroup getBranchGroup() { 89 | return branchGroup; 90 | } 91 | 92 | public void updateBranchGroup() { 93 | transform.setTranslation(position); 94 | transform.setRotationScale(rotation); 95 | transformGroup.setTransform(transform); 96 | } 97 | 98 | public void setIgnoreGravity(boolean ignoreGravity) { 99 | this.ignoreGravity = ignoreGravity; 100 | } 101 | 102 | public boolean isIgnoreWind() { 103 | return ignoreWind; 104 | } 105 | 106 | public void setIgnoreWind(boolean ignoreWind) { 107 | this.ignoreWind = ignoreWind; 108 | } 109 | 110 | public Vector3d getPosition() { 111 | return position; 112 | } 113 | 114 | public void setPosition(Vector3d position) { 115 | this.position = position; 116 | } 117 | 118 | public Vector3d getVelocity() { 119 | return velocity; 120 | } 121 | 122 | public void setVelocity(Vector3d vel) { 123 | this.velocity = vel; 124 | } 125 | 126 | public Vector3d getAcceleration() { 127 | return acceleration; 128 | } 129 | 130 | public void setAcceleration(Vector3d acc) { 131 | this.acceleration = acc; 132 | } 133 | 134 | public Matrix3d getRotation() { 135 | return rotation; 136 | } 137 | 138 | public void setRotation(Matrix3d rotation) { 139 | this.rotation = rotation; 140 | } 141 | 142 | public Vector3d getRotationRate() { 143 | return rotationRate; 144 | } 145 | 146 | public void setRotationRate(Vector3d rate) { 147 | this.rotationRate = rate; 148 | } 149 | 150 | public void resetObjectParameters() { 151 | position = new Vector3d(); 152 | velocity = new Vector3d(); 153 | acceleration = new Vector3d(); 154 | rotation = new Matrix3d(); 155 | rotationRate = new Vector3d(); 156 | 157 | rotation.rotX(0); 158 | } 159 | 160 | public static Vector3d utilMatrixToEulers(Matrix3d m) { 161 | Vector3d tv = new Vector3d(); 162 | tv.x = Math.atan2(m.m21, m.m22); 163 | tv.y = Math.asin(-m.m20); 164 | tv.z = Math.atan2(m.m10, m.m00); 165 | 166 | if (Math.abs(tv.y - Math.PI / 2) < 1e-3) { 167 | tv.x = 0; 168 | tv.z = Math.atan2(m.m12, m.m02); 169 | } else if (Math.abs(tv.y + Math.PI / 2) < 1e-3) { 170 | tv.x = 0; 171 | tv.z = Math.atan2(-m.m12, -m.m02); 172 | } 173 | return tv; 174 | } 175 | } 176 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/LogPlayerSensors.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.geo.LatLonAlt; 4 | import me.drton.jmavlib.log.FormatErrorException; 5 | import me.drton.jmavlib.log.LogReader; 6 | import me.drton.jmavlib.log.px4.PX4LogReader; 7 | 8 | import javax.vecmath.Vector3d; 9 | import java.io.EOFException; 10 | import java.io.IOException; 11 | import java.util.HashMap; 12 | import java.util.Map; 13 | 14 | /** 15 | * Sensors object that uses PX4 log file replay as source. 16 | */ 17 | public class LogPlayerSensors implements Sensors { 18 | private LogReader logReader = null; 19 | private long logStart = 0; 20 | private long logT = 0; 21 | private Vector3d acc = new Vector3d(); 22 | private Vector3d gyro = new Vector3d(); 23 | private Vector3d mag = new Vector3d(); 24 | private double baroAlt; 25 | private GNSSReport gnss = new GNSSReport(); 26 | private LatLonAlt globalPosition = new LatLonAlt(0, 0, 0); 27 | private boolean gpsUpdated = false; 28 | private boolean reset = false; 29 | 30 | void openLog(String fileName, long startTime) throws IOException, FormatErrorException { 31 | logReader = new PX4LogReader(fileName); 32 | logStart = startTime - logReader.getStartMicroseconds() / 1000; 33 | } 34 | 35 | @Override 36 | public void setObject(DynamicObject object, long t) { 37 | } 38 | 39 | @Override 40 | public void setGPSStartTime(long time) {} 41 | 42 | @Override 43 | public long getGPSStartTime() { 44 | return 0; 45 | } 46 | 47 | @Override 48 | public Vector3d getAcc() { 49 | return acc; 50 | } 51 | 52 | @Override 53 | public Vector3d getGyro() { 54 | return gyro; 55 | } 56 | 57 | @Override 58 | public Vector3d getMag() { 59 | return mag; 60 | } 61 | 62 | @Override 63 | public double getPressureAlt() { 64 | return baroAlt; 65 | } 66 | 67 | @Override 68 | public double getPressure() { 69 | return SimpleEnvironment.alt2baro(getPressureAlt()); 70 | } 71 | 72 | @Override 73 | public GNSSReport getGNSS() { 74 | return gnss; 75 | } 76 | 77 | @Override 78 | public LatLonAlt getGlobalPosition() { 79 | return globalPosition; 80 | } 81 | 82 | @Override 83 | public boolean isGPSUpdated() { 84 | boolean res = gpsUpdated; 85 | gpsUpdated = false; 86 | return res; 87 | } 88 | 89 | @Override 90 | public void update(long t, boolean paused) { 91 | if (logReader != null) { 92 | Map logData = new HashMap(); 93 | while (logStart + logT < t) { 94 | try { 95 | logT = logReader.readUpdate(logData) / 1000; 96 | } catch (EOFException e) { 97 | break; 98 | } catch (IOException e) { 99 | e.printStackTrace(); 100 | break; 101 | } catch (FormatErrorException e) { 102 | e.printStackTrace(); 103 | break; 104 | } 105 | } 106 | if (logData.containsKey("IMU.AccX") && 107 | logData.containsKey("IMU.AccY") && 108 | logData.containsKey("IMU.AccZ")) { 109 | acc.set((Float) logData.get("IMU.AccX"), (Float) logData.get("IMU.AccY"), 110 | (Float) logData.get("IMU.AccZ")); 111 | } 112 | if (logData.containsKey("IMU.GyroX") && 113 | logData.containsKey("IMU.GyroY") && 114 | logData.containsKey("IMU.GyroZ")) { 115 | gyro.set((Float) logData.get("IMU.GyroX"), (Float) logData.get("IMU.GyroY"), 116 | (Float) logData.get("IMU.GyroZ")); 117 | } 118 | if (logData.containsKey("IMU.MagX") && 119 | logData.containsKey("IMU.MagY") && 120 | logData.containsKey("IMU.MagZ")) { 121 | mag.set((Float) logData.get("IMU.MagX"), (Float) logData.get("IMU.MagY"), 122 | (Float) logData.get("IMU.MagZ")); 123 | } 124 | if (logData.containsKey("SENS.BaroAlt")) { 125 | baroAlt = (Float) logData.get("SENS.BaroAlt"); 126 | } 127 | if (logData.containsKey("GPS.Lat") && 128 | logData.containsKey("GPS.Lon") && 129 | logData.containsKey("GPS.Alt")) { 130 | gpsUpdated = true; 131 | gnss.position = new LatLonAlt(((Number) logData.get("GPS.Lat")).doubleValue(), 132 | ((Number) logData.get("GPS.Lon")).doubleValue(), 133 | ((Number) logData.get("GPS.Alt")).doubleValue()); 134 | gnss.eph = ((Number) logData.get("GPS.EPH")).floatValue(); 135 | gnss.epv = ((Number) logData.get("GPS.EPV")).floatValue(); 136 | gnss.velocity = new Vector3d(((Number) logData.get("GPS.VelN")).doubleValue(), 137 | ((Number) logData.get("GPS.VelE")).doubleValue(), 138 | ((Number) logData.get("GPS.VelD")).doubleValue()); 139 | gnss.fix = (Integer) logData.get("GPS.Fix"); 140 | gnss.time = (Long) logData.get("GPS.GPSTime"); 141 | 142 | globalPosition = gnss.position; 143 | } 144 | } 145 | } 146 | 147 | @Override 148 | public boolean isReset() { 149 | return reset; 150 | } 151 | 152 | @Override 153 | public void setReset(boolean reset) { 154 | this.reset = reset; 155 | } 156 | 157 | @Override 158 | public void setParameter(String name, float value) {} 159 | 160 | @Override 161 | public float param(String name) { 162 | return 0.0f; 163 | } 164 | 165 | } 166 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/LogPlayerTarget.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.geo.GlobalPositionProjector; 4 | import me.drton.jmavlib.geo.LatLonAlt; 5 | import me.drton.jmavlib.log.FormatErrorException; 6 | import me.drton.jmavlib.log.LogReader; 7 | 8 | import javax.vecmath.Vector3d; 9 | import java.io.EOFException; 10 | import java.io.FileNotFoundException; 11 | import java.io.IOException; 12 | import java.util.HashMap; 13 | import java.util.Map; 14 | 15 | /** 16 | * User: ton Date: 04.05.14 Time: 23:41 17 | */ 18 | public class LogPlayerTarget extends Target { 19 | private LogReader logReader = null; 20 | private long logStart = 0; 21 | private long timeStart = 0; 22 | private long logT = 0; 23 | private Vector3d positionOffset = new Vector3d(); 24 | private String[] posKeys = new String[] {"LPOS.X", "LPOS.Y", "LPOS.Z"}; 25 | private String[] velKeys = new String[] {"LPOS.VX", "LPOS.VY", "LPOS.VZ"}; 26 | private boolean globalFrame = false; 27 | private GlobalPositionProjector projector = null; 28 | private Vector3d postitionPrev = new Vector3d(); 29 | private long timePrev = 0; 30 | 31 | public LogPlayerTarget(World world, double size, boolean showGui) 32 | throws FileNotFoundException { 33 | super(world, size, showGui); 34 | } 35 | 36 | public void openLog(LogReader logReader) { 37 | this.logReader = logReader; 38 | logStart = timeStart - logReader.getStartMicroseconds() / 1000; 39 | } 40 | 41 | public void setLogKeys(String[] posKeys, String[] velKeys) { 42 | this.posKeys = posKeys; 43 | this.velKeys = velKeys; 44 | } 45 | 46 | public void setGlobalFrame(boolean globalFrame) { 47 | this.globalFrame = globalFrame; 48 | this.projector = new GlobalPositionProjector(); 49 | } 50 | 51 | public void setGlobalReference(LatLonAlt reference) { 52 | this.projector.init(reference); 53 | } 54 | 55 | public void setTimeStart(long timeStart) { 56 | this.timeStart = timeStart; 57 | } 58 | 59 | public void setPositionOffset(Vector3d positionOffset) { 60 | this.positionOffset = positionOffset; 61 | } 62 | 63 | @Override 64 | public void update(long t, boolean paused) { 65 | if (logReader != null) { 66 | Map logData = new HashMap(); 67 | while (logStart + logT < t) { 68 | try { 69 | logT = logReader.readUpdate(logData) / 1000; 70 | } catch (EOFException e) { 71 | break; 72 | } catch (IOException e) { 73 | e.printStackTrace(); 74 | break; 75 | } catch (FormatErrorException e) { 76 | e.printStackTrace(); 77 | break; 78 | } 79 | } 80 | if (logData.containsKey(posKeys[0]) && 81 | logData.containsKey(posKeys[1]) && 82 | logData.containsKey(posKeys[2])) { 83 | double[] v = new double[] { 84 | ((Number) logData.get(posKeys[0])).doubleValue(), 85 | ((Number) logData.get(posKeys[1])).doubleValue(), 86 | ((Number) logData.get(posKeys[2])).doubleValue() 87 | }; 88 | if (globalFrame) { 89 | LatLonAlt latLonAlt = new LatLonAlt(v[0], v[1], v[2]); 90 | if (!projector.isInited()) { 91 | projector.init(latLonAlt); 92 | } 93 | position.add(new Vector3d(projector.project(latLonAlt)), positionOffset); 94 | } else { 95 | position.add(new Vector3d(v), positionOffset); 96 | } 97 | if (velKeys == null) { 98 | // Calculate velocity from position changes 99 | velocity.sub(position, postitionPrev); 100 | velocity.scale(1000.0 / (logT - timePrev)); 101 | postitionPrev.set(position); 102 | timePrev = logT; 103 | } 104 | } 105 | if (velKeys != null) { 106 | // Use velocity from log 107 | if (logData.containsKey(velKeys[0]) && 108 | logData.containsKey(velKeys[1]) && 109 | logData.containsKey(velKeys[2])) { 110 | velocity.set((Float) logData.get(velKeys[0]), (Float) logData.get(velKeys[1]), 111 | (Float) logData.get(velKeys[2])); 112 | } 113 | } 114 | } 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkConnection.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 4 | 5 | import java.util.ArrayList; 6 | import java.util.HashSet; 7 | import java.util.List; 8 | import java.util.Set; 9 | 10 | /** 11 | * User: ton Date: 13.02.14 Time: 21:50 12 | */ 13 | public class MAVLinkConnection extends WorldObject { 14 | private List nodes = new ArrayList(); 15 | private Set skipMessages = new HashSet(); 16 | 17 | public MAVLinkConnection(World world) { 18 | super(world); 19 | } 20 | 21 | public void addNode(MAVLinkNode node) { 22 | nodes.add(node); 23 | node.addConnection(this); 24 | } 25 | 26 | public void addSkipMessage(int msgType) { 27 | skipMessages.add(msgType); 28 | } 29 | 30 | public void sendMessage(MAVLinkNode sender, MAVLinkMessage msg) { 31 | if (skipMessages.contains(msg.getMsgType())) { 32 | return; 33 | } 34 | for (MAVLinkNode node : nodes) { 35 | if (node != sender) { 36 | node.handleMessage(msg); 37 | } 38 | } 39 | } 40 | 41 | @Override 42 | public void update(long t, boolean paused) { 43 | for (MAVLinkNode node : nodes) { 44 | node.update(t, paused); 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkDisplayOnly.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.conversion.RotationConversion; 4 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 5 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 6 | import me.drton.jmavsim.vehicle.AbstractVehicle; 7 | 8 | import javax.vecmath.*; 9 | import java.util.Arrays; 10 | 11 | /** 12 | * MAVLinkDisplayOnly is MAVLink bridge between AbstractVehicle and autopilot connected via MAVLink. 13 | * MAVLinkDisplayOnly should have the same sysID as the autopilot, but different componentId. 14 | * It reads HIL_STATE_QUATERNION from the MAVLink and displays the vehicle position and attitude. 15 | * @author Romain Chiappinelli 16 | */ 17 | public class MAVLinkDisplayOnly extends MAVLinkHILSystemBase { 18 | 19 | private boolean firstMsg=true; // to detect the first MAVLink message 20 | private double lat0; // initial latitude (radians) 21 | private double lon0; // initial longitude (radians) 22 | private double alt0; // initial altitude (meters) 23 | private double lat; // geodetic latitude (radians) 24 | private double lon; // geodetic longitude (radians) 25 | private double alt; // above sea level (meters) 26 | private double [] quat={0.0,0.0,0.0,0.0}; // unit quaternion for attitude representation 27 | private Double [] control = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; 28 | private static final double EARTH_RADIUS=6371000.0; // earth radius in meters 29 | 30 | /** 31 | * Create MAVLinkDisplayOnly, MAVLink system that sends nothing to autopilot and passes states from 32 | * autopilot to simulator 33 | * 34 | * @param sysId SysId of simulator should be the same as autopilot 35 | * @param componentId ComponentId of simulator should be different from autopilot 36 | * @param vehicle vehicle to connect 37 | */ 38 | public MAVLinkDisplayOnly(MAVLinkSchema schema, int sysId, int componentId, AbstractVehicle vehicle) { 39 | super(schema, sysId, componentId, vehicle); 40 | } 41 | 42 | @Override 43 | public void handleMessage(MAVLinkMessage msg) { 44 | if ("HIL_STATE_QUATERNION".equals(msg.getMsgName())) { 45 | 46 | if (firstMsg) { 47 | firstMsg=false; 48 | // we take the first received position as initial position 49 | lat0=Math.toRadians(msg.getDouble("lat")*1e-7); 50 | lon0=Math.toRadians(msg.getDouble("lon")*1e-7); 51 | alt0=msg.getDouble("alt")*1e-3; 52 | } 53 | for (int i = 0; i < 4; ++i) { 54 | quat[i] = ((Number)((Object[])msg.get("attitude_quaternion"))[i]).doubleValue(); 55 | } 56 | lat=Math.toRadians(msg.getDouble("lat")*1e-7); 57 | lon=Math.toRadians(msg.getDouble("lon")*1e-7); 58 | alt=msg.getDouble("alt")*1e-3; 59 | 60 | Vector3d pos = new Vector3d(EARTH_RADIUS*(lat-lat0),EARTH_RADIUS*(lon-lon0)*Math.cos(lat0),alt0-alt); 61 | double [] euler = RotationConversion.eulerAnglesByQuaternion(quat); 62 | Matrix3d dcm = new Matrix3d(RotationConversion.rotationMatrixByEulerAngles(euler[0],euler[1],euler[2])); 63 | 64 | vehicle.setControl(Arrays.asList(control)); // set 0 throttles 65 | vehicle.setPosition(pos); // we want ideally a "local" pos groundtruth 66 | vehicle.setRotation(dcm); 67 | } 68 | } 69 | 70 | @Override 71 | public boolean gotHilActuatorControls() 72 | { 73 | return !firstMsg; 74 | } 75 | 76 | @Override 77 | public void initMavLink() 78 | { 79 | firstMsg=true; 80 | } 81 | 82 | @Override 83 | public void endSim() 84 | { 85 | 86 | } 87 | 88 | } 89 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkHILSystem.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.conversion.RotationConversion; 4 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 5 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 6 | import me.drton.jmavsim.vehicle.AbstractVehicle; 7 | 8 | import javax.vecmath.*; 9 | import java.util.Arrays; 10 | import java.util.Collections; 11 | import java.util.List; 12 | import java.util.ArrayList; 13 | import java.util.Calendar; 14 | import java.util.TimeZone; 15 | 16 | /** 17 | * MAVLinkHILSystem is MAVLink bridge between AbstractVehicle and autopilot connected via MAVLink. 18 | * MAVLinkHILSystem should have the same sysID as the autopilot, but different componentId. 19 | */ 20 | public class MAVLinkHILSystem extends MAVLinkHILSystemBase { 21 | // private Simulator simulator; 22 | // private AbstractVehicle vehicle; 23 | private boolean gotHeartBeat = false; 24 | private boolean inited = false; 25 | private boolean stopped = false; 26 | private boolean gotHilActuatorControls = false; 27 | private long hilStateUpdateInterval = -1; //don't publish by default 28 | private long nextHilStatePub = 0; 29 | private long timeThrottleCounter = 0; 30 | private long lastHeartbeatMs = 0; 31 | 32 | /** 33 | * Create MAVLinkHILSimulator, MAVLink system that sends simulated sensors to autopilot and passes controls from 34 | * autopilot to simulator 35 | * 36 | * @param sysId SysId of simulator should be the same as autopilot 37 | * @param componentId ComponentId of simulator should be different from autopilot 38 | * @param vehicle vehicle to connect 39 | */ 40 | public MAVLinkHILSystem(MAVLinkSchema schema, int sysId, int componentId, AbstractVehicle vehicle) { 41 | super(schema, sysId, componentId, vehicle); 42 | } 43 | 44 | @Override 45 | public boolean gotHilActuatorControls() { 46 | return gotHilActuatorControls; 47 | } 48 | 49 | @Override 50 | public void handleMessage(MAVLinkMessage msg) { 51 | super.handleMessage(msg); 52 | long t = simulator.getSimMillis(); 53 | if ("HIL_ACTUATOR_CONTROLS".equals(msg.getMsgName())) { 54 | gotHilActuatorControls = true; 55 | List control = new ArrayList(); 56 | for (int i = 0; i < 8; ++i) { 57 | control.add(((Number)((Object[])msg.get("controls"))[i]).doubleValue()); 58 | } 59 | 60 | // Get the system arming state if the mode 61 | // field is valid 62 | int mode = msg.getInt("mode"); 63 | boolean armed = true; 64 | 65 | if (mode != 0) { 66 | if ((mode & 128) > 0 /* armed */) { 67 | armed = true; 68 | } else { 69 | armed = false; 70 | } 71 | } 72 | 73 | simulator.advanceTime(); 74 | 75 | vehicle.setControl(control); 76 | 77 | } else if ("HIL_CONTROLS".equals(msg.getMsgName()) && 78 | !gotHilActuatorControls) { //this is deprecated, but we still support it for now 79 | List control = Arrays.asList(msg.getDouble("roll_ailerons"), 80 | msg.getDouble("pitch_elevator"), 81 | msg.getDouble("yaw_rudder"), msg.getDouble("throttle"), msg.getDouble("aux1"), 82 | msg.getDouble("aux2"), msg.getDouble("aux3"), msg.getDouble("aux4")); 83 | 84 | // Get the system arming state if the mode 85 | // field is valid 86 | int mode = msg.getInt("mode"); 87 | boolean armed = true; 88 | 89 | if (mode != 0) { 90 | if ((mode & 128) > 0 /* armed */) { 91 | armed = true; 92 | } else { 93 | armed = false; 94 | } 95 | } 96 | 97 | vehicle.setControl(control); 98 | 99 | } else if ("COMMAND_LONG".equals(msg.getMsgName())) { 100 | int command = msg.getInt("command"); 101 | if (command == 511) { //MAV_CMD_SET_MESSAGE_INTERVAL 102 | int msg_id = (int)(msg.getFloat("param1") + 0.5); 103 | if (msg_id == 115) { //HIL_STATE_QUATERNION 104 | hilStateUpdateInterval = (int)(msg.getFloat("param2") + 0.5); 105 | } 106 | } 107 | } else if ("HEARTBEAT".equals(msg.getMsgName())) { 108 | long realMs = simulator.getRealMillis(); 109 | 110 | // We timeout after 3 seconds and do a reset. 111 | long diffMs = realMs - lastHeartbeatMs; 112 | if (diffMs > 3000) { 113 | if (gotHeartBeat) { 114 | System.out.println("Reseting after silence of " + diffMs + " ms"); 115 | } 116 | reset(); 117 | } 118 | 119 | if (!gotHeartBeat && !stopped) { 120 | if (sysId < 0 || sysId == msg.systemID) { 121 | gotHeartBeat = true; 122 | if (sysId < 0) { 123 | sysId = msg.systemID; 124 | } 125 | 126 | System.out.println("Init MAVLink"); 127 | initMavLink(); 128 | 129 | } else if (sysId > -1 && sysId != msg.systemID) { 130 | System.out.println("WARNING: Got heartbeat from system #" + Integer.toString(msg.systemID) + 131 | " but configured to only accept messages from system #" + Integer.toString(sysId) + 132 | ". Please change the system ID parameter to match in order to use HITL/SITL."); 133 | } 134 | } 135 | if ((msg.getInt("base_mode") & 128) == 0) { 136 | vehicle.setControl(Collections.emptyList()); 137 | } 138 | 139 | lastHeartbeatMs = realMs; 140 | 141 | } else if ("STATUSTEXT".equals(msg.getMsgName())) { 142 | System.out.println("MSG: " + msg.getString("text")); 143 | } 144 | } 145 | 146 | @Override 147 | public void initMavLink() { 148 | if (vehicle.getSensors().getGPSStartTime() == -1) { 149 | vehicle.getSensors().setGPSStartTime(simulator.getSimMillis() + 1000); 150 | } 151 | stopped = false; 152 | inited = true; 153 | } 154 | 155 | @Override 156 | public void endSim() { 157 | if (!inited) { 158 | return; 159 | } 160 | inited = false; 161 | gotHeartBeat = false; 162 | stopped = true; 163 | vehicle.getSensors().setGPSStartTime(-1); 164 | } 165 | 166 | @Override 167 | public void update(long t, boolean paused) { 168 | super.update(t, paused); 169 | 170 | if (paused) { 171 | return; 172 | } 173 | 174 | long tu = t * 1000; // Time in us 175 | 176 | if (!this.inited) { 177 | return; 178 | } 179 | 180 | Sensors sensors = vehicle.getSensors(); 181 | 182 | // Sensors 183 | MAVLinkMessage msg_sensor = new MAVLinkMessage(schema, "HIL_SENSOR", sysId, componentId, 184 | protocolVersion); 185 | 186 | // sensor source bitmask 187 | int sensor_source = 0; 188 | 189 | msg_sensor.set("time_usec", tu); 190 | Vector3d tv = sensors.getAcc(); 191 | msg_sensor.set("xacc", tv.x); 192 | msg_sensor.set("yacc", tv.y); 193 | msg_sensor.set("zacc", tv.z); 194 | tv = sensors.getGyro(); 195 | sensor_source |= 0b111; 196 | msg_sensor.set("xgyro", tv.x); 197 | msg_sensor.set("ygyro", tv.y); 198 | msg_sensor.set("zgyro", tv.z); 199 | tv = sensors.getMag(); 200 | sensor_source |= 0b111000; 201 | msg_sensor.set("xmag", tv.x); 202 | msg_sensor.set("ymag", tv.y); 203 | msg_sensor.set("zmag", tv.z); 204 | sensor_source |= 0b111000000; 205 | msg_sensor.set("pressure_alt", sensors.getPressureAlt()); 206 | msg_sensor.set("abs_pressure", sensors.getPressure() * 0.01); // Pa to millibar 207 | sensor_source |= 0b1101000000000; 208 | if (sensors.isReset()) { 209 | msg_sensor.set("fields_updated", (1 << 31)); 210 | sensors.setReset(false); 211 | } else { 212 | msg_sensor.set("fields_updated", sensor_source); 213 | } 214 | sendMessage(msg_sensor); 215 | 216 | /* ground truth */ 217 | if (hilStateUpdateInterval != -1 && nextHilStatePub <= tu) { 218 | MAVLinkMessage msg_hil_state = new MAVLinkMessage(schema, "HIL_STATE_QUATERNION", sysId, 219 | componentId, protocolVersion); 220 | msg_hil_state.set("time_usec", tu); 221 | 222 | Float[] q = RotationConversion.quaternionByEulerAngles(vehicle.attitude); 223 | msg_hil_state.set("attitude_quaternion", q); 224 | 225 | Vector3d v3d = vehicle.getRotationRate(); 226 | msg_hil_state.set("rollspeed", (float) v3d.x); 227 | msg_hil_state.set("pitchspeed", (float) v3d.y); 228 | msg_hil_state.set("yawspeed", (float) v3d.z); 229 | 230 | int alt = (int)(1000 * vehicle.position.z); 231 | msg_hil_state.set("alt", alt); 232 | msg_hil_state.set("lat", (int)(sensors.getGlobalPosition().lat * 1.e7)); 233 | msg_hil_state.set("lon", (int)(sensors.getGlobalPosition().lon * 1.e7)); 234 | 235 | v3d = vehicle.getVelocity(); 236 | msg_hil_state.set("vx", (int)(v3d.x * 100)); 237 | msg_hil_state.set("vy", (int)(v3d.y * 100)); 238 | msg_hil_state.set("vz", (int)(v3d.z * 100)); 239 | 240 | Vector3d airSpeed = new Vector3d(vehicle.getVelocity()); 241 | airSpeed.scale(-1.0); 242 | airSpeed.add(vehicle.getWorld().getEnvironment().getCurrentWind(vehicle.position)); 243 | float as_mag = (float) airSpeed.length(); 244 | msg_hil_state.set("true_airspeed", (int)(as_mag * 100)); 245 | 246 | v3d = vehicle.acceleration; 247 | msg_hil_state.set("xacc", (int)(v3d.x * 1000)); 248 | msg_hil_state.set("yacc", (int)(v3d.y * 1000)); 249 | msg_hil_state.set("zacc", (int)(v3d.z * 1000)); 250 | 251 | sendMessage(msg_hil_state); 252 | nextHilStatePub = tu + hilStateUpdateInterval; 253 | } 254 | 255 | // GPS 256 | if (sensors.isGPSUpdated()) { 257 | GNSSReport gps = sensors.getGNSS(); 258 | if (gps != null && gps.position != null && gps.velocity != null) { 259 | MAVLinkMessage msg_gps = new MAVLinkMessage(schema, "HIL_GPS", sysId, componentId, protocolVersion); 260 | msg_gps.set("time_usec", tu); 261 | msg_gps.set("lat", (long)(gps.position.lat * 1e7)); 262 | msg_gps.set("lon", (long)(gps.position.lon * 1e7)); 263 | msg_gps.set("alt", (long)(gps.position.alt * 1e3)); 264 | msg_gps.set("vn", (int)(gps.velocity.x * 100)); 265 | msg_gps.set("ve", (int)(gps.velocity.y * 100)); 266 | msg_gps.set("vd", (int)(gps.velocity.z * 100)); 267 | msg_gps.set("eph", (int)(gps.eph * 100f)); 268 | msg_gps.set("epv", (int)(gps.epv * 100f)); 269 | msg_gps.set("vel", (int)(gps.getSpeed() * 100)); 270 | msg_gps.set("cog", (int) Math.toDegrees(gps.getCog()) * 100); 271 | msg_gps.set("fix_type", gps.fix); 272 | msg_gps.set("satellites_visible", 10); 273 | sendMessage(msg_gps); 274 | } 275 | } 276 | 277 | // SYSTEM TIME from host 278 | if (timeThrottleCounter++ % 1000 == 0) { 279 | MAVLinkMessage msg_system_time = new MAVLinkMessage(schema, "SYSTEM_TIME", sysId, componentId, 280 | protocolVersion); 281 | Calendar cal = Calendar.getInstance(TimeZone.getTimeZone("GMT")); 282 | msg_system_time.set("time_unix_usec", cal.getTimeInMillis() * 1000); 283 | msg_system_time.set("time_boot_ms", tu / 1000); 284 | sendMessage(msg_system_time); 285 | } 286 | } 287 | 288 | private void reset() { 289 | gotHeartBeat = false; 290 | inited = false; 291 | stopped = false; 292 | gotHilActuatorControls = false; 293 | nextHilStatePub = 0; 294 | } 295 | } 296 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkHILSystemBase.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 4 | import me.drton.jmavsim.vehicle.AbstractVehicle; 5 | 6 | /** 7 | * MAVLinkHILSystemBase is the superclass of MAVLinkHILSystem and MAVLinkDisplayOnly. It is a bridge between 8 | * AbstractVehicle and autopilot connected via MAVLink. 9 | * MAVLinkHILSystemBase should have the same sysID as the autopilot, but different componentId. 10 | */ 11 | public abstract class MAVLinkHILSystemBase extends MAVLinkSystem { 12 | protected Simulator simulator; 13 | protected AbstractVehicle vehicle; 14 | 15 | /** 16 | * Create MAVLinkHILSimulator, MAVLink system that sends simulated sensors to autopilot and passes controls from 17 | * autopilot to simulator 18 | * 19 | * @param sysId SysId of simulator should be the same as autopilot 20 | * @param componentId ComponentId of simulator should be different from autopilot 21 | * @param vehicle vehicle to connect 22 | */ 23 | public MAVLinkHILSystemBase(MAVLinkSchema schema, int sysId, int componentId, AbstractVehicle vehicle) { 24 | super(schema, sysId, componentId); 25 | this.vehicle = vehicle; 26 | } 27 | 28 | public void setSimulator(Simulator simulator) { 29 | this.simulator = simulator; 30 | } 31 | 32 | public abstract boolean gotHilActuatorControls(); 33 | 34 | public abstract void initMavLink(); 35 | 36 | public abstract void endSim(); 37 | 38 | } 39 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkMissionItem.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | /** 4 | * User: ton Date: 22.05.14 Time: 17:31 5 | */ 6 | public class MAVLinkMissionItem { 7 | public final int command; 8 | public final int frame; 9 | public final float param1; 10 | public final float param2; 11 | public final float param3; 12 | public final float param4; 13 | public final float x; 14 | public final float y; 15 | public final float z; 16 | public final int autocontinue; 17 | 18 | public MAVLinkMissionItem(int frame, int command, float param1, float param2, float param3, 19 | float param4, float x, 20 | float y, float z, int autocontinue) { 21 | this.frame = frame; 22 | this.command = command; 23 | this.param1 = param1; 24 | this.param2 = param2; 25 | this.param3 = param3; 26 | this.param4 = param4; 27 | this.x = x; 28 | this.y = y; 29 | this.z = z; 30 | this.autocontinue = autocontinue; 31 | } 32 | 33 | @Override 34 | public String toString() { 35 | return String.format( 36 | "", 37 | frame, command, param1, param2, param3, param4, x, y, z, autocontinue); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkNode.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 4 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 5 | 6 | import java.util.ArrayList; 7 | import java.util.List; 8 | 9 | /** 10 | * MAVLinkNode is generic object that can handle and send MAVLink messages, but may have no own ID, i.e. it can be e.g. 11 | * bridge between physical port and virtual MAVLinkConnection. 12 | *

13 | * User: ton Date: 13.02.14 Time: 21:51 14 | */ 15 | public abstract class MAVLinkNode { 16 | protected MAVLinkSchema schema; 17 | private List connections = new ArrayList(); 18 | 19 | protected MAVLinkNode(MAVLinkSchema schema) { 20 | this.schema = schema; 21 | } 22 | 23 | public void addConnection(MAVLinkConnection connection) { 24 | connections.add(connection); 25 | } 26 | 27 | protected void sendMessage(MAVLinkMessage msg) { 28 | for (MAVLinkConnection connection : connections) { 29 | connection.sendMessage(this, msg); 30 | } 31 | } 32 | 33 | public abstract void handleMessage(MAVLinkMessage msg); 34 | 35 | public abstract void update(long t, boolean paused); 36 | } 37 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkPort.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 4 | 5 | import java.io.IOException; 6 | 7 | /** 8 | * User: ton Date: 02.12.13 Time: 20:56 9 | */ 10 | public abstract class MAVLinkPort extends MAVLinkNode { 11 | protected MAVLinkPort(MAVLinkSchema schema) { 12 | super(schema); 13 | } 14 | 15 | public abstract void open() throws IOException; 16 | 17 | public abstract void close() throws IOException; 18 | 19 | public abstract boolean isOpened(); 20 | 21 | public abstract void setDebug(boolean debug); 22 | } 23 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MAVLinkSystem.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 4 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 5 | 6 | /** 7 | * MAVLinkSystem represents generic MAVLink system with SysID and ComponentID that can handle and send messages. 8 | *

9 | * User: ton Date: 13.02.14 Time: 20:32 10 | */ 11 | public class MAVLinkSystem extends MAVLinkNode { 12 | public int sysId; 13 | public int componentId; 14 | private long heartbeatInterval = 1000; // [ms] 15 | private long heartbeatNext = 0; 16 | protected int protocolVersion = 1; 17 | 18 | public MAVLinkSystem(MAVLinkSchema schema, int sysId, int componentId) { 19 | super(schema); 20 | this.sysId = sysId; 21 | this.componentId = componentId; 22 | } 23 | 24 | public void setHeartbeatInterval(long interval) { 25 | this.heartbeatInterval = interval; 26 | } 27 | 28 | @Override 29 | public void handleMessage(MAVLinkMessage msg) { 30 | // Update our mavlink version according to incoming messages. 31 | protocolVersion = msg.protocolVersion; 32 | } 33 | 34 | @Override 35 | public void update(long t, boolean paused) { 36 | if (heartbeatNext <= t && heartbeatInterval > 0) { 37 | MAVLinkMessage msg = new MAVLinkMessage(schema, "HEARTBEAT", sysId, componentId, protocolVersion); 38 | msg.set("mavlink_version", 3); 39 | sendMessage(msg); 40 | heartbeatNext = t + heartbeatInterval; 41 | } 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/MavlinkTest.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 4 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 5 | import org.xml.sax.SAXException; 6 | 7 | import javax.xml.parsers.ParserConfigurationException; 8 | import java.io.IOException; 9 | 10 | /** 11 | * User: ton Date: 21.03.14 Time: 13:44 12 | */ 13 | public class MavlinkTest { 14 | public static void main(String[] args) 15 | throws InterruptedException, IOException, ParserConfigurationException, SAXException { 16 | World world = new World(); 17 | MAVLinkSchema schema = new MAVLinkSchema("mavlink/message_definitions/common.xml"); 18 | MAVLinkConnection connection = new MAVLinkConnection(world); 19 | SerialMAVLinkPort port = new SerialMAVLinkPort(schema); 20 | connection.addNode(port); 21 | MAVLinkNode node = new MAVLinkNode(schema) { 22 | @Override 23 | public void handleMessage(MAVLinkMessage msg) { 24 | System.out.println(msg); 25 | } 26 | 27 | @Override 28 | public void update(long t, boolean paused) { 29 | } 30 | }; 31 | connection.addNode(node); 32 | port.setup("/dev/tty.usbmodem1", 57600, 8, 1, 0); 33 | port.open(); 34 | port.sendRaw("\nsh /etc/init.d/rc.usb\n".getBytes()); 35 | while (true) { 36 | port.update(System.currentTimeMillis(), false); 37 | Thread.sleep(10); 38 | } 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/PeripherialBuzzer.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import java.util.concurrent.BlockingQueue; 4 | import java.util.concurrent.LinkedBlockingQueue; 5 | 6 | import javax.sound.sampled.AudioFormat; 7 | import javax.sound.sampled.AudioSystem; 8 | import javax.sound.sampled.LineUnavailableException; 9 | import javax.sound.sampled.SourceDataLine; 10 | 11 | public class PeripherialBuzzer { 12 | BlockingQueue notes = new LinkedBlockingQueue<>(); 13 | AudioFormat af; 14 | SourceDataLine sdl; 15 | float sample_rate = 44100; // assume a sample rate of 44.1 kHz 16 | 17 | class Note { 18 | public int frequency; 19 | public int duration; 20 | public Note(int frequency, int duration) { 21 | this.frequency = Integer.valueOf(frequency); 22 | this.duration = duration; 23 | } 24 | } 25 | 26 | public void playNote(int note, int duration) { 27 | notes.add(new Note(note, duration)); 28 | } 29 | 30 | public class NoteConsumer implements Runnable { 31 | public void run() { 32 | while (true) { 33 | try { 34 | Note timedNote = notes.take(); 35 | generateTone(timedNote.frequency, timedNote.duration); 36 | } catch (LineUnavailableException e) { 37 | e.printStackTrace(); 38 | } catch (InterruptedException e) { 39 | e.printStackTrace(); 40 | } 41 | } 42 | } 43 | } 44 | 45 | public PeripherialBuzzer() { 46 | Thread myThread = new Thread(new NoteConsumer()); 47 | af = new AudioFormat(sample_rate, 8, 1, true, false); 48 | try { 49 | sdl = AudioSystem.getSourceDataLine(af); 50 | sdl.open(af); 51 | } catch (LineUnavailableException e) { 52 | e.printStackTrace(); 53 | } 54 | 55 | sdl.start(); 56 | 57 | myThread.start(); 58 | } 59 | 60 | public void generateTone(int hz, int msecs) throws LineUnavailableException { 61 | byte[] buf = new byte[2048]; 62 | 63 | int k = 0; 64 | 65 | for (int i = 0; i < msecs * sample_rate / 1000;) { 66 | // fill up the local buffer 67 | for (k = 0; k < 2047 && (i < msecs * sample_rate / 1000); i++, k++) { 68 | buf[k] = (byte)(Math.sin(i / (sample_rate / hz) * 2.0 * Math.PI) * 100); 69 | } 70 | 71 | sdl.write(buf, 0, k); 72 | } 73 | 74 | sdl.drain(); 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/ReportPanel.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import java.awt.*; 4 | 5 | /** 6 | * A UI panel containing the simulation report. 7 | */ 8 | public class ReportPanel extends Panel { 9 | private static final long serialVersionUID = 8196526002006067676L; 10 | private final TextArea textArea; 11 | 12 | public ReportPanel() { 13 | super(new BorderLayout()); 14 | 15 | textArea = new TextArea("", 0, 0, TextArea.SCROLLBARS_VERTICAL_ONLY); 16 | textArea.setEditable(false); 17 | setIsFocusable(false); 18 | textArea.setFont(new Font("monospaced", Font.PLAIN, 12)); 19 | textArea.setPreferredSize(new Dimension(300, 0)); 20 | 21 | add("Center", textArea); 22 | } 23 | 24 | public void setText(String report) { 25 | textArea.setText(report); 26 | } 27 | 28 | public void setIsFocusable(boolean on) { 29 | textArea.setFocusable(on); 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/ReportUpdater.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | /** 4 | * Updater for the visualizer's simulation state's report. 5 | */ 6 | public class ReportUpdater extends WorldObject { 7 | private static final long UPDATE_FREQ_MS = 250; 8 | 9 | private static final StringBuilder builder = new StringBuilder(); 10 | private static long updateFreq; 11 | private static long nextUpdateT; 12 | private final Visualizer3D visualizer; 13 | 14 | 15 | public ReportUpdater(World world, Visualizer3D visualizer) { 16 | super(world); 17 | this.visualizer = visualizer; 18 | setUpdateFreq(UPDATE_FREQ_MS); 19 | } 20 | 21 | public static long getUpdateFreq() { 22 | return ReportUpdater.updateFreq; 23 | } 24 | 25 | public static void setUpdateFreq(long updateFreq) { 26 | ReportUpdater.updateFreq = updateFreq; 27 | ReportUpdater.nextUpdateT = System.currentTimeMillis() + updateFreq; 28 | } 29 | 30 | public static void resetUpdateFreq() { 31 | setUpdateFreq(UPDATE_FREQ_MS); 32 | } 33 | 34 | @Override 35 | public void update(long t, boolean paused) { 36 | if (t < nextUpdateT) { 37 | return; 38 | } 39 | 40 | nextUpdateT = t + updateFreq; 41 | 42 | if (!visualizer.showReportText()) { 43 | return; 44 | } 45 | 46 | builder.setLength(0); 47 | 48 | for (WorldObject object : getWorld().getObjects()) { 49 | if (object instanceof ReportingObject) { 50 | ((ReportingObject) object).report(builder); 51 | } 52 | } 53 | 54 | visualizer.setReportText(builder.toString()); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/ReportUtil.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import javax.vecmath.Vector3d; 4 | 5 | /** 6 | * A class containing helper methods for producing simulation reports. 7 | */ 8 | public final class ReportUtil { 9 | public static final String ff = "%+010.5f; "; 10 | // private static final DecimalFormat df = new DecimalFormat("0.#####", DecimalFormatSymbols.getInstance(Locale.ENGLISH)); 11 | 12 | private ReportUtil() { 13 | } 14 | 15 | /** 16 | * Converts a vector to a shorter string. 17 | * When numbers are changing quickly on the screen, there may not be time to read the E notation. 18 | */ 19 | public static String vector2str(Vector3d vec) { 20 | return String.format(ff + ff + ff, vec.x, vec.y, vec.z); 21 | } 22 | 23 | /** 24 | * Converts a double to a string. 25 | */ 26 | public static String d2str(double val) { 27 | return String.format(ff, val); 28 | } 29 | 30 | /** 31 | * Converts a vector of radians to a vector of degrees 32 | */ 33 | public static Vector3d vectRad2Deg(Vector3d v) { 34 | return new Vector3d(Math.toDegrees(v.x), Math.toDegrees(v.y), Math.toDegrees(v.z)); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/ReportingObject.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | /** 4 | * An object that can write its textual representation to a string builder. 5 | */ 6 | public interface ReportingObject { 7 | String newLine = System.getProperty("line.separator"); 8 | 9 | void report(StringBuilder builder); 10 | } 11 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/Rotor.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | /** 4 | * Simple rotor model. Thrust and torque are proportional to control signal filtered with simple LPF (RC filter), to 5 | * simulate spin up/slow down. 6 | */ 7 | public class Rotor { 8 | private double tau = 1.0; 9 | private double fullThrust = 1.0; 10 | private double fullTorque = 1.0; 11 | private double w = 0.0; 12 | private long lastTime = -1; 13 | private double control = 0.0; 14 | 15 | public void update(long t, boolean paused) { 16 | if (paused) { 17 | return; 18 | } 19 | 20 | if (lastTime >= 0) { 21 | double dt = (t - lastTime) / 1000.0; 22 | w += (control - w) * (1.0 - Math.exp(-dt / tau)); 23 | } 24 | lastTime = t; 25 | } 26 | 27 | /** 28 | * Set control signal 29 | * @param control control signal normalized to [0...1] for traditional or [-1...1] for reversable rotors 30 | */ 31 | public void setControl(double control) { 32 | this.control = control; 33 | } 34 | 35 | /** 36 | * Set full thrust 37 | * @param fullThrust [N] 38 | */ 39 | public void setFullThrust(double fullThrust) { 40 | this.fullThrust = fullThrust; 41 | } 42 | 43 | /** 44 | * Set torque at full thrust 45 | * @param fullTorque [N * m] 46 | */ 47 | public void setFullTorque(double fullTorque) { 48 | this.fullTorque = fullTorque; 49 | } 50 | 51 | /** 52 | * Set time constant (spin-up time). 53 | * @param timeConstant [s] 54 | */ 55 | public void setTimeConstant(double timeConstant) { 56 | this.tau = timeConstant; 57 | } 58 | 59 | /** 60 | * Get control signal 61 | */ 62 | public double getControl() { 63 | return control; 64 | } 65 | 66 | /** 67 | * Get current rotor thrust, [N] 68 | */ 69 | public double getThrust() { 70 | return w * fullThrust; 71 | } 72 | 73 | /** 74 | * Get current rotor torque [N * m] 75 | */ 76 | public double getTorque() { 77 | return control * fullTorque; 78 | } 79 | 80 | /** 81 | * Get full thrust, [N] 82 | */ 83 | public double getFullThrust() { 84 | return fullThrust; 85 | } 86 | 87 | /** 88 | * Get torque at full thrust, [N * m] 89 | */ 90 | public double getFullTorque() { 91 | return fullTorque; 92 | } 93 | 94 | /** 95 | * Get time constant (spin-up time), [s]. 96 | */ 97 | public double getTimeConstant() { 98 | return tau; 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SensorParamPanel.form: -------------------------------------------------------------------------------- 1 | 2 |

3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 |
114 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SensorParamPanel.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import com.intellij.uiDesigner.core.GridConstraints; 4 | import com.intellij.uiDesigner.core.GridLayoutManager; 5 | 6 | import javax.swing.*; 7 | import javax.swing.event.ChangeEvent; 8 | import javax.swing.event.ChangeListener; 9 | import java.awt.*; 10 | 11 | /** 12 | * @author SungTae Moon 13 | * @file SensorParamPanel.java 14 | * Sensor Control Parameter Panel 15 | *

16 | * This panel is used for the sensor test and analysis 17 | */ 18 | 19 | public class SensorParamPanel extends JPanel { 20 | private JSpinner accelSpinner; 21 | private JSpinner gyroSpinner; 22 | private JPanel mainPanel; 23 | private JSpinner magSpinner; 24 | private JSpinner presSpinner; 25 | private JSpinner gpsSpinner; 26 | private JSpinner massSpinner; 27 | 28 | protected Sensors sensors = null; 29 | 30 | public SensorParamPanel() { 31 | 32 | this.add(mainPanel); 33 | 34 | accelSpinner.setModel(new SpinnerNumberModel(0.0f, 0.0f, 1.0f, 0.01f)); 35 | gyroSpinner.setModel(new SpinnerNumberModel(0.0f, 0.0f, 1.0f, 0.01f)); 36 | gpsSpinner.setModel(new SpinnerNumberModel(0.0f, 0.0f, 100.0f, 1.0f)); 37 | magSpinner.setModel(new SpinnerNumberModel(0.0f, 0.0f, 1.0f, 0.001f)); 38 | presSpinner.setModel(new SpinnerNumberModel(0.0f, 0.0f, 1.0f, 0.01f)); 39 | massSpinner.setModel(new SpinnerNumberModel(0.0f, 0.0f, 5.0f, 0.1f)); 40 | 41 | accelSpinner.addChangeListener(new ChangeListener() { 42 | @Override 43 | public void stateChanged(ChangeEvent e) { 44 | Double value = (Double) accelSpinner.getValue(); 45 | sensors.setParameter("noise_Acc", value.floatValue()); 46 | } 47 | }); 48 | 49 | gyroSpinner.addChangeListener(new ChangeListener() { 50 | @Override 51 | public void stateChanged(ChangeEvent e) { 52 | Double value = (Double) gyroSpinner.getValue(); 53 | sensors.setParameter("noise_Gyo", value.floatValue()); 54 | } 55 | }); 56 | 57 | magSpinner.addChangeListener(new ChangeListener() { 58 | @Override 59 | public void stateChanged(ChangeEvent e) { 60 | Double value = (Double) magSpinner.getValue(); 61 | sensors.setParameter("noise_Mag", value.floatValue()); 62 | } 63 | }); 64 | 65 | presSpinner.addChangeListener(new ChangeListener() { 66 | @Override 67 | public void stateChanged(ChangeEvent e) { 68 | Double value = (Double) presSpinner.getValue(); 69 | sensors.setParameter("noise_Prs", value.floatValue()); 70 | } 71 | }); 72 | 73 | gpsSpinner.addChangeListener(new ChangeListener() { 74 | @Override 75 | public void stateChanged(ChangeEvent e) { 76 | Double value = (Double) gpsSpinner.getValue(); 77 | sensors.setParameter("gpsNoiseStdDev", value.floatValue()); 78 | } 79 | }); 80 | 81 | massSpinner.addChangeListener(new ChangeListener() { 82 | @Override 83 | public void stateChanged(ChangeEvent e) { 84 | Double value = (Double) massSpinner.getValue(); 85 | sensors.setParameter("mass", value.floatValue()); 86 | } 87 | }); 88 | 89 | } 90 | 91 | public JPanel panel() { 92 | return mainPanel; 93 | } 94 | 95 | public void setSensor(Sensors sensors) { 96 | this.sensors = sensors; 97 | 98 | // init value 99 | accelSpinner.setValue(Double.valueOf(sensors.param("noise_Acc"))); 100 | gyroSpinner.setValue(Double.valueOf(sensors.param("noise_Gyo"))); 101 | magSpinner.setValue(Double.valueOf(sensors.param("noise_Mag"))); 102 | presSpinner.setValue(Double.valueOf(sensors.param("noise_Prs"))); 103 | gpsSpinner.setValue(Double.valueOf(sensors.param("gpsNoiseStdDev"))); 104 | massSpinner.setValue(Double.valueOf(sensors.param("mass"))); 105 | 106 | } 107 | 108 | { 109 | // GUI initializer generated by IntelliJ IDEA GUI Designer 110 | // >>> IMPORTANT!! <<< 111 | // DO NOT EDIT OR ADD ANY CODE HERE! 112 | $$$setupUI$$$(); 113 | } 114 | 115 | /** 116 | * Method generated by IntelliJ IDEA GUI Designer 117 | * >>> IMPORTANT!! <<< 118 | * DO NOT edit this method OR call it in your code! 119 | * 120 | * @noinspection ALL 121 | */ 122 | private void $$$setupUI$$$() { 123 | mainPanel = new JPanel(); 124 | mainPanel.setLayout(new GridLayoutManager(8, 2, new Insets(0, 0, 0, 0), -1, -1)); 125 | final JLabel label1 = new JLabel(); 126 | label1.setText("Accel Noise StdDev"); 127 | mainPanel.add(label1, new GridConstraints(1, 0, 1, 1, GridConstraints.ANCHOR_WEST, 128 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 129 | null, null, 0, false)); 130 | accelSpinner = new JSpinner(); 131 | mainPanel.add(accelSpinner, new GridConstraints(1, 1, 1, 1, GridConstraints.ANCHOR_WEST, 132 | GridConstraints.FILL_HORIZONTAL, GridConstraints.SIZEPOLICY_WANT_GROW, 133 | GridConstraints.SIZEPOLICY_FIXED, null, null, null, 0, false)); 134 | final JLabel label2 = new JLabel(); 135 | label2.setText("Gyro Noise StdDev"); 136 | mainPanel.add(label2, new GridConstraints(2, 0, 1, 1, GridConstraints.ANCHOR_WEST, 137 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 138 | null, null, 0, false)); 139 | gyroSpinner = new JSpinner(); 140 | mainPanel.add(gyroSpinner, new GridConstraints(2, 1, 1, 1, GridConstraints.ANCHOR_WEST, 141 | GridConstraints.FILL_HORIZONTAL, GridConstraints.SIZEPOLICY_WANT_GROW, 142 | GridConstraints.SIZEPOLICY_FIXED, null, null, null, 0, false)); 143 | final JLabel label3 = new JLabel(); 144 | label3.setText("Mag Noise StdDev"); 145 | mainPanel.add(label3, new GridConstraints(3, 0, 1, 1, GridConstraints.ANCHOR_WEST, 146 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 147 | null, null, 0, false)); 148 | final JLabel label4 = new JLabel(); 149 | label4.setText("Pressure Noise StdDev"); 150 | mainPanel.add(label4, new GridConstraints(4, 0, 1, 1, GridConstraints.ANCHOR_WEST, 151 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 152 | null, null, 0, false)); 153 | final JLabel label5 = new JLabel(); 154 | label5.setText("GPS Noise StdDev"); 155 | mainPanel.add(label5, new GridConstraints(5, 0, 1, 1, GridConstraints.ANCHOR_WEST, 156 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 157 | null, null, 0, false)); 158 | magSpinner = new JSpinner(); 159 | mainPanel.add(magSpinner, new GridConstraints(3, 1, 1, 1, GridConstraints.ANCHOR_WEST, 160 | GridConstraints.FILL_HORIZONTAL, GridConstraints.SIZEPOLICY_WANT_GROW, 161 | GridConstraints.SIZEPOLICY_FIXED, null, null, null, 0, false)); 162 | presSpinner = new JSpinner(); 163 | mainPanel.add(presSpinner, new GridConstraints(4, 1, 1, 1, GridConstraints.ANCHOR_WEST, 164 | GridConstraints.FILL_HORIZONTAL, GridConstraints.SIZEPOLICY_WANT_GROW, 165 | GridConstraints.SIZEPOLICY_FIXED, null, null, null, 0, false)); 166 | gpsSpinner = new JSpinner(); 167 | mainPanel.add(gpsSpinner, new GridConstraints(5, 1, 1, 1, GridConstraints.ANCHOR_WEST, 168 | GridConstraints.FILL_HORIZONTAL, GridConstraints.SIZEPOLICY_WANT_GROW, 169 | GridConstraints.SIZEPOLICY_FIXED, null, null, null, 0, false)); 170 | final JLabel label6 = new JLabel(); 171 | label6.setText("--- VEHICLE ---"); 172 | mainPanel.add(label6, new GridConstraints(6, 0, 1, 2, GridConstraints.ANCHOR_CENTER, 173 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 174 | null, null, 0, false)); 175 | final JLabel label7 = new JLabel(); 176 | label7.setText("mass"); 177 | mainPanel.add(label7, new GridConstraints(7, 0, 1, 1, GridConstraints.ANCHOR_WEST, 178 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 179 | null, null, 0, false)); 180 | massSpinner = new JSpinner(); 181 | mainPanel.add(massSpinner, new GridConstraints(7, 1, 1, 1, GridConstraints.ANCHOR_WEST, 182 | GridConstraints.FILL_HORIZONTAL, GridConstraints.SIZEPOLICY_WANT_GROW, 183 | GridConstraints.SIZEPOLICY_FIXED, null, null, null, 0, false)); 184 | final JLabel label8 = new JLabel(); 185 | label8.setText("--- SENSORS ---"); 186 | mainPanel.add(label8, new GridConstraints(0, 0, 1, 2, GridConstraints.ANCHOR_CENTER, 187 | GridConstraints.FILL_NONE, GridConstraints.SIZEPOLICY_FIXED, GridConstraints.SIZEPOLICY_FIXED, null, 188 | null, null, 0, false)); 189 | } 190 | 191 | /** 192 | * @noinspection ALL 193 | */ 194 | public JComponent $$$getRootComponent$$$() { 195 | return mainPanel; 196 | } 197 | } 198 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/Sensors.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import javax.vecmath.Vector3d; 4 | 5 | import me.drton.jmavlib.geo.LatLonAlt; 6 | 7 | /** 8 | * User: ton Date: 26.11.13 Time: 13:32 9 | */ 10 | public interface Sensors { 11 | void setObject(DynamicObject object, long t); 12 | 13 | Vector3d getAcc(); 14 | 15 | Vector3d getGyro(); 16 | 17 | Vector3d getMag(); 18 | 19 | double getPressureAlt(); 20 | 21 | double getPressure(); 22 | 23 | GNSSReport getGNSS(); 24 | 25 | LatLonAlt getGlobalPosition(); 26 | 27 | boolean isGPSUpdated(); 28 | 29 | boolean isReset(); 30 | 31 | void setReset(boolean reset); 32 | 33 | void setGPSStartTime(long time); 34 | 35 | long getGPSStartTime(); 36 | 37 | void update(long t, boolean paused); 38 | 39 | void setParameter(String name, float value); 40 | 41 | float param(String name); 42 | 43 | } 44 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SerialMAVLinkPort.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import jssc.SerialPort; 4 | import jssc.SerialPortException; 5 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 6 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 7 | import me.drton.jmavlib.mavlink.MAVLinkStream; 8 | 9 | import java.io.IOException; 10 | import java.nio.ByteBuffer; 11 | import java.nio.channels.ByteChannel; 12 | 13 | /** 14 | * User: ton Date: 28.11.13 Time: 23:30 15 | */ 16 | public class SerialMAVLinkPort extends MAVLinkPort { 17 | private MAVLinkSchema schema = null; 18 | private SerialPort serialPort = null; 19 | private ByteChannel channel = null; 20 | private MAVLinkStream stream = null; 21 | private boolean debug = false; 22 | 23 | // connection information 24 | String portName; 25 | int baudRate; 26 | int dataBits; 27 | int stopBits; 28 | int parity; 29 | 30 | public SerialMAVLinkPort(MAVLinkSchema schema) { 31 | super(schema); 32 | this.schema = schema; 33 | } 34 | 35 | public void setup(String portName, int baudRate, int dataBits, int stopBits, int parity) { 36 | this.portName = portName; 37 | this.baudRate = baudRate; 38 | this.dataBits = dataBits; 39 | this.stopBits = stopBits; 40 | this.parity = parity; 41 | } 42 | 43 | public void setDebug(boolean debug) { 44 | this.debug = debug; 45 | } 46 | 47 | public void open() throws IOException { 48 | serialPort = new SerialPort(portName); 49 | try { 50 | serialPort.openPort(); 51 | serialPort.setParams(baudRate, dataBits, stopBits, parity); 52 | } catch (SerialPortException e) { 53 | serialPort = null; 54 | throw new IOException(e); 55 | } 56 | channel = new ByteChannel() { 57 | @Override 58 | public int read(ByteBuffer buffer) throws IOException { 59 | try { 60 | int available = serialPort.getInputBufferBytesCount(); 61 | if (available <= 0) { 62 | return 0; 63 | } 64 | 65 | byte[] b = serialPort.readBytes(Math.min(available, buffer.remaining())); 66 | if (b != null) { 67 | buffer.put(b); 68 | return b.length; 69 | } 70 | 71 | return 0; 72 | 73 | } catch (SerialPortException e) { 74 | throw new IOException(e); 75 | } 76 | } 77 | 78 | @Override 79 | public int write(ByteBuffer buffer) throws IOException { 80 | try { 81 | byte[] b = new byte[buffer.remaining()]; 82 | buffer.get(b); 83 | return serialPort.writeBytes(b) ? b.length : 0; 84 | } catch (SerialPortException e) { 85 | throw new IOException(e); 86 | } 87 | } 88 | 89 | @Override 90 | public boolean isOpen() { 91 | return serialPort.isOpened(); 92 | } 93 | 94 | @Override 95 | public void close() throws IOException { 96 | try { 97 | serialPort.closePort(); 98 | } catch (SerialPortException e) { 99 | throw new IOException(e); 100 | } 101 | } 102 | }; 103 | stream = new MAVLinkStream(schema, channel); 104 | stream.setDebug(debug); 105 | } 106 | 107 | @Override 108 | public void close() throws IOException { 109 | if (serialPort == null) { 110 | return; 111 | } 112 | 113 | try { 114 | serialPort.closePort(); 115 | } catch (SerialPortException e) { 116 | throw new IOException(e); 117 | } 118 | serialPort = null; 119 | stream = null; 120 | } 121 | 122 | @Override 123 | public boolean isOpened() { 124 | return serialPort != null && serialPort.isOpened(); 125 | } 126 | 127 | @Override 128 | public void handleMessage(MAVLinkMessage msg) { 129 | if (isOpened() && stream != null) { 130 | try { 131 | stream.write(msg); 132 | } catch (Exception e) { 133 | e.printStackTrace(); 134 | } 135 | } 136 | } 137 | 138 | @Override 139 | public void update(long t, boolean paused) { 140 | MAVLinkMessage msg; 141 | while (isOpened() && stream != null) { 142 | try { 143 | msg = stream.read(); 144 | if (msg == null) { 145 | break; 146 | } 147 | msg.forwarded = true; 148 | sendMessage(msg); 149 | } catch (IOException e) { 150 | e.printStackTrace(); 151 | return; 152 | } 153 | } 154 | } 155 | 156 | public void sendRaw(byte[] data) throws IOException { 157 | try { 158 | serialPort.writeBytes(data); 159 | } catch (SerialPortException e) { 160 | throw new IOException(e); 161 | } 162 | } 163 | } 164 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SerialPortChannel.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import jssc.SerialPort; 4 | import jssc.SerialPortException; 5 | 6 | import java.io.IOException; 7 | import java.nio.ByteBuffer; 8 | import java.nio.channels.ByteChannel; 9 | 10 | public class SerialPortChannel implements ByteChannel { 11 | private final SerialPort serialPort; 12 | 13 | public SerialPortChannel(SerialPort serialPort) { 14 | this.serialPort = serialPort; 15 | } 16 | 17 | @Override 18 | public int read(ByteBuffer buffer) throws IOException { 19 | try { 20 | int available = serialPort.getInputBufferBytesCount(); 21 | if (available <= 0) { 22 | return 0; 23 | } 24 | byte[] b = serialPort.readBytes(Math.min(available, buffer.remaining())); 25 | if (b != null) { 26 | buffer.put(b); 27 | return b.length; 28 | } else { 29 | return 0; 30 | } 31 | } catch (SerialPortException e) { 32 | throw new IOException(e); 33 | } 34 | } 35 | 36 | @Override 37 | public int write(ByteBuffer buffer) throws IOException { 38 | try { 39 | byte[] b = new byte[buffer.remaining()]; 40 | buffer.get(b); 41 | return serialPort.writeBytes(b) ? b.length : 0; 42 | } catch (SerialPortException e) { 43 | throw new IOException(e); 44 | } 45 | } 46 | 47 | @Override 48 | public boolean isOpen() { 49 | return serialPort.isOpened(); 50 | } 51 | 52 | @Override 53 | public void close() throws IOException { 54 | try { 55 | serialPort.closePort(); 56 | } catch (SerialPortException e) { 57 | throw new IOException(e); 58 | } 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SimpleEnvironment.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import javax.vecmath.Vector3d; 4 | 5 | import java.util.Random; 6 | 7 | /** 8 | * User: ton Date: 28.11.13 Time: 22:40 9 | */ 10 | public class SimpleEnvironment extends Environment { 11 | public static final double Pb = 101325.0; // static pressure at sea level [Pa] 12 | public static final double Tb = 288.15; // standard temperature at sea level [K] 13 | public static final double Lb = -0.0065; // standard temperature lapse rate [K/m] 14 | public static final double M = 0.0289644; // molar mass of Earth's air [kg/mol] 15 | public static final double G = 9.80665; // gravity 16 | public static final double R = 8.31432; // universal gas constant 17 | 18 | private Random random = new Random(); 19 | private long lastTime = 0; 20 | 21 | public SimpleEnvironment(World world) { 22 | super(world); 23 | setG(null); 24 | setMagField(new Vector3d(0.21523, 0.0, 0.42741)); 25 | } 26 | 27 | @Override 28 | public void setG(Vector3d grav) { 29 | if (grav == null) { 30 | g = new Vector3d(0.0, 0.0, G); 31 | } else { 32 | g = grav; 33 | } 34 | } 35 | 36 | public void update(long t, boolean paused) { 37 | if (paused) { 38 | return; 39 | } 40 | double dt = lastTime == 0 ? 0.0 : (t - lastTime) / 1000.0; 41 | lastTime = t; 42 | Vector3d r = new Vector3d(windDeviation); 43 | r.scale(random.nextGaussian()); 44 | Vector3d dev = new Vector3d(wind); 45 | dev.sub(windCurrent); 46 | dev.scale(1.0 / windT); 47 | r.add(dev); 48 | r.scale(dt); 49 | windCurrent.add(r); 50 | } 51 | 52 | 53 | /** 54 | * Convert altitude to barometric pressure 55 | * 56 | * @param alt Altitude in meters 57 | * 58 | * @return Barometric pressure in Pa 59 | */ 60 | public static double alt2baro(double alt) { 61 | if (alt <= 11000.0) { 62 | return Pb * Math.pow(Tb / (Tb + (Lb * alt)), (G * M) / (R * Lb)); 63 | } else if (alt <= 20000.0) { 64 | double f = 11000.0; 65 | double a = alt2baro(f); 66 | double c = Tb + (f * Lb); 67 | return a * Math.exp(((-G) * M * (alt - f)) / (R * c)); 68 | } 69 | return 0.0; 70 | } 71 | 72 | 73 | // Mag declination calculator 74 | 75 | /** set this always to the sampling in degrees for the table below */ 76 | private int SAMPLING_RES = 10; 77 | private int SAMPLING_MIN_LAT = -60; 78 | private int SAMPLING_MAX_LAT = 60; 79 | private int SAMPLING_MIN_LON = -180; 80 | private int SAMPLING_MAX_LON = 180; 81 | 82 | private int[][] declination_table = { 83 | { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, 84 | { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, 85 | { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, 86 | { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, 87 | { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, 88 | { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, 89 | { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, 90 | { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, 91 | { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, 92 | { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, 93 | { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, 94 | { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, 95 | { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, 96 | }; 97 | 98 | private double get_lookup_table_val(int lat_index, int lon_index) { 99 | return declination_table[lat_index][lon_index]; 100 | } 101 | 102 | /** 103 | * Get the mag declination at this point 104 | * 105 | * @param lat latitude in degrees 106 | * @param lon longitude in degrees 107 | * @return mag declination in degrees 108 | */ 109 | public double getMagDeclination(double lat, double lon) { 110 | /* 111 | * If the values exceed valid ranges, return zero as default 112 | * as we have no way of knowing what the closest real value 113 | * would be. 114 | */ 115 | if (lat < -90.0f || lat > 90.0f || 116 | lon < -180.0f || lon > 180.0f) { 117 | return 0.0f; 118 | } 119 | 120 | /* round down to nearest sampling resolution */ 121 | int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; 122 | int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; 123 | 124 | /* for the rare case of hitting the bounds exactly 125 | * the rounding logic wouldn't fit, so enforce it. 126 | */ 127 | 128 | /* limit to table bounds - required for maxima even when table spans full globe range */ 129 | if (lat <= SAMPLING_MIN_LAT) { 130 | min_lat = SAMPLING_MIN_LAT; 131 | } 132 | 133 | if (lat >= SAMPLING_MAX_LAT) { 134 | min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; 135 | } 136 | 137 | if (lon <= SAMPLING_MIN_LON) { 138 | min_lon = SAMPLING_MIN_LON; 139 | } 140 | 141 | if (lon >= SAMPLING_MAX_LON) { 142 | min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; 143 | } 144 | 145 | /* find index of nearest low sampling point */ 146 | int min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; 147 | int min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; 148 | 149 | double declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); 150 | double declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); 151 | double declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); 152 | double declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); 153 | 154 | /* perform bilinear interpolation on the four grid corners */ 155 | 156 | double declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + 157 | declination_sw; 158 | double declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + 159 | declination_nw; 160 | 161 | return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; 162 | } 163 | 164 | } 165 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SimpleSensors.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.geo.GlobalPositionProjector; 4 | import me.drton.jmavlib.geo.LatLonAlt; 5 | import me.drton.jmavlib.processing.DelayLine; 6 | 7 | import javax.vecmath.Matrix3d; 8 | import javax.vecmath.Vector3d; 9 | 10 | /** 11 | * User: ton Date: 27.11.13 Time: 19:06 12 | */ 13 | public class SimpleSensors implements Sensors { 14 | private DynamicObject object; 15 | private GlobalPositionProjector globalProjector = new GlobalPositionProjector(); 16 | private DelayLine gpsDelayLine = new DelayLine(); 17 | private long gpsStartTime = -1; 18 | private long gpsInterval = 200; // [ms] 19 | private long gpsNext = 0; 20 | private GNSSReport gps = new GNSSReport(); 21 | private LatLonAlt globalPosition = new LatLonAlt(0, 0, 0); 22 | private boolean gpsUpdated = false; 23 | private boolean reset = false; 24 | private double pressureAltOffset = 0.0; 25 | // default sensor output noise levels 26 | private float noise_Acc = 0.05f; 27 | private float noise_Gyo = 0.01f; 28 | private float noise_Mag = 0.005f; 29 | private float noise_Prs = 0.01f; 30 | private double magScale = 1.0; // scaling factor for mag sensor 31 | private float ephHigh = 100.0f; // starting GPS horizontal estimation accuracy 32 | private float ephLow = 0.3f; // final GPS horizontal estimation accuracy 33 | private float epvHigh = 100.0f; // starting GPS vertical estimation accuracy 34 | private float epvLow = 0.4f; // final GPS vertical estimation accuracy 35 | private float fix3Deph = 3.0f; // maximum h-acc for a "3D" fix 36 | private float fix2Deph = 4.0f; // maximum h-acc for a "2D" fix 37 | // gps noise 38 | private float gpsNoiseStdDev = 10.f; 39 | private double randomWalkGpsX = 0.0; 40 | private double randomWalkGpsY = 0.0; 41 | private double randomWalkGpsZ = 0.0; 42 | private double gpsCorrelationTime = 30.0; 43 | private long prevUpdateTime = 0; 44 | // accuracy smoothing filters, slowly improve h/v accuracy after startup 45 | private Filter ephFilter = new Filter(); 46 | private Filter epvFilter = new Filter(); 47 | 48 | public SimpleSensors() { 49 | initFilters(); 50 | } 51 | 52 | private void initFilters() { 53 | ephFilter.filterInit(1.0 / gpsInterval, 0.9, ephHigh); 54 | epvFilter.filterInit(1.0 / gpsInterval, 0.9, epvHigh); 55 | } 56 | 57 | @Override 58 | public void setObject(DynamicObject object, long t) { 59 | this.object = object; 60 | globalProjector.init(object.getWorld().getGlobalReference()); 61 | setGlobalPosition(null, t); 62 | } 63 | 64 | public void setGPSStartTime(long time) { 65 | gpsStartTime = time; 66 | } 67 | 68 | public long getGPSStartTime() { 69 | return gpsStartTime; 70 | } 71 | 72 | public void setGPSDelay(long delay) { 73 | gpsDelayLine.setDelay(delay); 74 | } 75 | 76 | public void setGPSInterval(long gpsInterval) { 77 | this.gpsInterval = gpsInterval; 78 | // re-init filters with new dt 79 | initFilters(); 80 | } 81 | 82 | public void setPressureAltOffset(double pressureAltOffset) { 83 | this.pressureAltOffset = pressureAltOffset; 84 | } 85 | 86 | public void setNoise_Acc(float noise_Acc) { 87 | this.noise_Acc = noise_Acc; 88 | } 89 | 90 | public void setNoise_Gyo(float noise_Gyo) { 91 | this.noise_Gyo = noise_Gyo; 92 | } 93 | 94 | public void setNoise_Mag(float noise_Mag) { 95 | this.noise_Mag = noise_Mag; 96 | } 97 | 98 | public void setNoise_Prs(float noise_Prs) { 99 | this.noise_Prs = noise_Prs; 100 | } 101 | 102 | public double getMagScale() { 103 | return magScale; 104 | } 105 | 106 | public void setMagScale(double magScale) { 107 | this.magScale = magScale; 108 | } 109 | 110 | public boolean isReset() { 111 | return reset; 112 | } 113 | 114 | public void setReset(boolean reset) { 115 | this.reset = reset; 116 | } 117 | 118 | @Override 119 | public Vector3d getAcc() { 120 | Vector3d accBody = new Vector3d(object.getAcceleration()); 121 | accBody.sub(object.getWorld().getEnvironment().getG()); 122 | Matrix3d rot = new Matrix3d(object.getRotation()); 123 | rot.transpose(); 124 | rot.transform(accBody); 125 | accBody = addZeroMeanNoise(accBody, noise_Acc); 126 | return accBody; 127 | } 128 | 129 | @Override 130 | public Vector3d getGyro() { 131 | return addZeroMeanNoise(object.getRotationRate(), noise_Gyo); 132 | } 133 | 134 | @Override 135 | public Vector3d getMag() { 136 | Vector3d mag = new Vector3d(object.getWorld().getEnvironment().getMagField(object.getPosition())); 137 | Matrix3d rot = new Matrix3d(object.getRotation()); 138 | rot.transpose(); 139 | rot.transform(mag); 140 | mag.scale(magScale); 141 | return addZeroMeanNoise(mag, noise_Mag); 142 | } 143 | 144 | @Override 145 | public double getPressureAlt() { 146 | return getGlobalPosition().alt + randomNoise(noise_Prs) + pressureAltOffset; 147 | } 148 | 149 | @Override 150 | public double getPressure() { 151 | return SimpleEnvironment.alt2baro(getPressureAlt()); 152 | } 153 | 154 | @Override 155 | public GNSSReport getGNSS() { 156 | return gps; 157 | } 158 | 159 | @Override 160 | public LatLonAlt getGlobalPosition() { 161 | return globalPosition; 162 | } 163 | 164 | public void setGlobalPosition(Vector3d pos, long t) { 165 | if (pos == null) { 166 | pos = object.getPosition(); 167 | } 168 | 169 | double dt = 0.0; 170 | if (prevUpdateTime > 0) { 171 | dt = (t - this.prevUpdateTime) * 1e-3; 172 | } 173 | 174 | // add noise (random walk) 175 | if (dt > 0.0) { 176 | double sqrtDt = java.lang.Math.sqrt(dt); 177 | double noiseX = sqrtDt * randomNoise(gpsNoiseStdDev); 178 | double noiseY = sqrtDt * randomNoise(gpsNoiseStdDev); 179 | double noiseZ = sqrtDt * randomNoise(gpsNoiseStdDev); 180 | 181 | this.randomWalkGpsX += noiseX * dt - this.randomWalkGpsX / gpsCorrelationTime; 182 | this.randomWalkGpsY += noiseY * dt - this.randomWalkGpsY / gpsCorrelationTime; 183 | this.randomWalkGpsZ += noiseZ * dt - this.randomWalkGpsZ / gpsCorrelationTime; 184 | } 185 | 186 | double noiseGpsX = pos.x + this.randomWalkGpsX; 187 | double noiseGpsY = pos.y + this.randomWalkGpsY; 188 | double noiseGpsZ = pos.z + this.randomWalkGpsZ; 189 | 190 | globalPosition = globalProjector.reproject(new double[] {noiseGpsX, noiseGpsY, noiseGpsZ}); 191 | 192 | this.prevUpdateTime = t; 193 | } 194 | 195 | @Override 196 | public boolean isGPSUpdated() { 197 | boolean res = gpsUpdated; 198 | gpsUpdated = false; 199 | return res; 200 | } 201 | 202 | @Override 203 | public void update(long t, boolean paused) { 204 | if (paused) { 205 | return; 206 | } 207 | 208 | float eph, epv; 209 | setGlobalPosition(null, t); 210 | 211 | // GPS 212 | if (gpsStartTime > -1 && t > gpsStartTime && gpsNext <= t) { 213 | gpsNext = t + gpsInterval; 214 | gpsUpdated = true; 215 | GNSSReport gpsCurrent = new GNSSReport(); 216 | eph = (float)ephFilter.filter(ephLow); 217 | epv = (float)epvFilter.filter(epvLow); 218 | 219 | gpsCurrent.position = globalPosition; 220 | gpsCurrent.eph = eph; 221 | gpsCurrent.epv = epv; 222 | gpsCurrent.velocity = new Vector3d(object.getVelocity()); 223 | gpsCurrent.fix = eph <= fix3Deph ? 3 : eph <= fix2Deph ? 2 : 0; 224 | gpsCurrent.time = t * 1000; 225 | gps = gpsDelayLine.getOutput(t, gpsCurrent); 226 | } 227 | } 228 | 229 | @Override 230 | public void setParameter(String name, float value) { 231 | if (name.equals("noise_Acc")) { 232 | noise_Acc = value; 233 | } else if (name.equals("noise_Gyo")) { 234 | noise_Gyo = value; 235 | } else if (name.equals("noise_Mag")) { 236 | noise_Mag = value; 237 | } else if (name.equals("noise_Prs")) { 238 | noise_Prs = value; 239 | } else if (name.equals("gpsNoiseStdDev")) { 240 | gpsNoiseStdDev = value; 241 | } else if (name.equals("mass")) { 242 | object.setMass((double)value); 243 | } else { 244 | System.out.printf("ERROR: unknown param"); 245 | } 246 | } 247 | 248 | @Override 249 | public float param(String name) { 250 | if (name.equals("noise_Acc")) { 251 | return noise_Acc; 252 | } else if (name.equals("noise_Gyo")) { 253 | return noise_Gyo; 254 | } else if (name.equals("noise_Mag")) { 255 | return noise_Mag; 256 | } else if (name.equals("noise_Prs")) { 257 | return noise_Prs; 258 | } else if (name.equals("gpsNoiseStdDev")) { 259 | return gpsNoiseStdDev; 260 | } else if (name.equals("mass")) { 261 | return (float)object.getMass(); 262 | } else { 263 | System.out.printf("ERROR: unknown param"); 264 | } 265 | 266 | return 0.0f; 267 | } 268 | 269 | // Utility methods 270 | 271 | public double randomNoise(float stdDev) { 272 | double x0; 273 | double b0, b1; 274 | 275 | do { 276 | b0 = Math.random(); 277 | b1 = Math.random(); 278 | } while (b0 <= Float.intBitsToFloat(0x1)); 279 | 280 | x0 = java.lang.Math.sqrt(-2.0 * java.lang.Math.log(b0)) * java.lang.Math.cos(Math.PI * 2.0 * b1); 281 | 282 | if (java.lang.Double.isInfinite(x0) || java.lang.Double.isNaN(x0)) { 283 | x0 = 0.0; 284 | } 285 | 286 | return x0 * stdDev; 287 | } 288 | 289 | public Vector3d addZeroMeanNoise(Vector3d vIn, float stdDev) { 290 | 291 | return new Vector3d(vIn.x + randomNoise(stdDev), 292 | vIn.y + randomNoise(stdDev), 293 | vIn.z + randomNoise(stdDev)); 294 | } 295 | 296 | } 297 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SimpleTarget.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import javax.vecmath.Vector3d; 4 | import java.io.FileNotFoundException; 5 | 6 | /** 7 | * User: ton Date: 04.05.14 Time: 23:25 8 | */ 9 | public class SimpleTarget extends Target { 10 | private Vector3d positionStart = new Vector3d(); 11 | private Vector3d positionFinish = new Vector3d(); 12 | private long timeStart = 0; 13 | private long timeFinish = 10000; 14 | 15 | public SimpleTarget(World world, double size, boolean showGui) 16 | throws FileNotFoundException { 17 | super(world, size, showGui); 18 | } 19 | 20 | public void setTrajectory(Vector3d positionStart, Vector3d positionFinish, long timeStart, 21 | long timeFinish) { 22 | this.positionStart = positionStart; 23 | this.positionFinish = positionFinish; 24 | this.timeStart = timeStart; 25 | this.timeFinish = timeFinish; 26 | } 27 | 28 | @Override 29 | public void update(long t, boolean paused) { 30 | double progress = Math.min(1.0, Math.max(0.0, 31 | (double)(t - timeStart) / (double)(timeFinish - timeStart))); 32 | Vector3d vec = new Vector3d(); 33 | vec.sub(positionFinish, positionStart); 34 | position.scaleAdd(progress, vec, positionStart); 35 | if (progress > 0.0 && progress < 1.0) { 36 | velocity.scale(1000.0 / (timeFinish - timeStart), vec); 37 | } else { 38 | velocity.set(0.0, 0.0, 0.0); 39 | } 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/SystemOutHandler.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | //import java.io.ByteArrayOutputStream; 4 | import java.io.IOException; 5 | import java.io.OutputStream; 6 | import java.io.PrintStream; 7 | import java.util.ArrayList; 8 | import java.util.List; 9 | 10 | public class SystemOutHandler { 11 | private PrintStream osPrintStream; 12 | private SystemOutHandlerOutputStream os; 13 | private boolean active; 14 | private boolean logToStdOut; 15 | 16 | public SystemOutHandler(boolean logToStdOut) { 17 | this.active = false; 18 | this.logToStdOut = logToStdOut; 19 | this.osPrintStream = System.out; 20 | this.os = new SystemOutHandlerOutputStream(new ArrayList(1)); 21 | } 22 | 23 | public void setLogToStdOut(boolean logToStdOut) { 24 | this.logToStdOut = logToStdOut; 25 | } 26 | 27 | public void addOutputStream(OutputStream os) { 28 | this.os.addStream(os); 29 | } 30 | 31 | public void removeOutputStream(OutputStream os) { 32 | this.os.removeStream(os); 33 | } 34 | 35 | public void start(boolean clearStreams) { 36 | if (this.active) { 37 | return; 38 | } 39 | 40 | this.active = true; 41 | if (clearStreams) { 42 | os.clearStreams(); 43 | } 44 | if (this.logToStdOut) { 45 | os.addStream(this.osPrintStream); 46 | } 47 | 48 | System.setOut(new PrintStream(this.os)); 49 | } 50 | 51 | public void stop() { 52 | this.active = false; 53 | System.setOut(osPrintStream); 54 | } 55 | 56 | private static class SystemOutHandlerOutputStream extends OutputStream { 57 | private List outputStreams; 58 | 59 | public SystemOutHandlerOutputStream(List outputStreams) { 60 | this.outputStreams = outputStreams; 61 | } 62 | 63 | public void addStream(OutputStream os) { 64 | if (!this.outputStreams.contains(os)) { 65 | this.outputStreams.add(os); 66 | } 67 | } 68 | 69 | public void removeStream(OutputStream os) { 70 | if (this.outputStreams.contains(os)) { 71 | this.outputStreams.remove(os); 72 | } 73 | } 74 | 75 | public void clearStreams() { 76 | this.outputStreams.clear(); 77 | } 78 | 79 | public void write(int b) throws IOException { 80 | for (OutputStream os : this.outputStreams) { 81 | if (os != null) { 82 | os.write(b); 83 | } 84 | } 85 | } 86 | 87 | public void flush() throws IOException { 88 | for (OutputStream os : this.outputStreams) { 89 | if (os != null) { 90 | os.flush(); 91 | } 92 | } 93 | } 94 | 95 | public void close() throws IOException { 96 | for (OutputStream os : outputStreams) { 97 | if (os != null) { 98 | os.close(); 99 | } 100 | } 101 | } 102 | } 103 | } -------------------------------------------------------------------------------- /src/me/drton/jmavsim/TCPMavLinkPort.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 4 | import me.drton.jmavlib.mavlink.MAVLinkStream; 5 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 6 | 7 | import java.io.IOException; 8 | import java.net.*; 9 | import java.nio.channels.ServerSocketChannel; 10 | import java.nio.channels.SocketChannel; 11 | import java.nio.ByteBuffer; 12 | import java.nio.channels.ByteChannel; 13 | import java.util.*; 14 | 15 | /** 16 | * User: ton Date: 02.12.13 Time: 20:56 17 | */ 18 | public class TCPMavLinkPort extends MAVLinkPort { 19 | private MAVLinkSchema schema; 20 | private ByteBuffer rxBuffer = ByteBuffer.allocate(8192); 21 | private InetSocketAddress inetSocketAddress = null; 22 | private ServerSocketChannel serverSocketChannel = null; 23 | private SocketChannel socketChannel = null; 24 | private MAVLinkStream stream; 25 | private boolean debug = false; 26 | 27 | private boolean monitorMessage = false; 28 | private HashSet monitorMessageIDs; 29 | private HashMap messageCounts = new HashMap(); 30 | 31 | static int MONITOR_MESSAGE_RATE = 100; // rate at which to print message info 32 | static int TIME_PASSING = 10; // change the print so it's visible to the user. 33 | static int time = 0; 34 | 35 | 36 | public TCPMavLinkPort(MAVLinkSchema schema) { 37 | super(schema); 38 | this.schema = schema; 39 | rxBuffer.flip(); 40 | } 41 | 42 | public void setMonitorMessageID(HashSet ids) { 43 | this.monitorMessageIDs = ids; 44 | for (int id : ids) { 45 | messageCounts.put(id, 0); 46 | } 47 | this.monitorMessage = true; 48 | } 49 | 50 | public void setDebug(boolean debug) { 51 | this.debug = debug; 52 | } 53 | 54 | public void setup(String address, int port) throws UnknownHostException, IOException { 55 | inetSocketAddress = new InetSocketAddress(port); 56 | } 57 | 58 | public void open() throws IOException { 59 | serverSocketChannel = ServerSocketChannel.open(); 60 | serverSocketChannel.socket().bind(inetSocketAddress); 61 | accept(); 62 | stream = new MAVLinkStream(schema, socketChannel); 63 | stream.setDebug(true); 64 | } 65 | 66 | private void accept() { 67 | if (debug) { 68 | System.out.println("Waiting to accept TCP connection"); 69 | } 70 | 71 | try { 72 | socketChannel = serverSocketChannel.accept(); 73 | socketChannel.configureBlocking(false); 74 | socketChannel.setOption(StandardSocketOptions.TCP_NODELAY, true); 75 | } catch (IOException ignored) { 76 | } 77 | 78 | if (debug) { 79 | System.out.println("TCP connection accepted"); 80 | } 81 | } 82 | 83 | private void reset() { 84 | if (debug) { 85 | System.out.println("Reseting TCP connection."); 86 | } 87 | 88 | try { 89 | close(); 90 | open(); 91 | } catch (IOException e) { 92 | System.err.println("Reset failed: " + e); 93 | } 94 | } 95 | 96 | @Override 97 | public void close() throws IOException { 98 | serverSocketChannel.close(); 99 | } 100 | 101 | @Override 102 | public boolean isOpened() { 103 | return serverSocketChannel != null && serverSocketChannel.isOpen(); 104 | } 105 | 106 | @Override 107 | public void handleMessage(MAVLinkMessage msg) { 108 | if (debug) { 109 | System.out.println("[handleMessage] msg.name: " + msg.getMsgName() + ", type: " + msg.getMsgType()); 110 | } 111 | 112 | if (isOpened()) { 113 | try { 114 | stream.write(msg); 115 | IndicateReceivedMessage(msg.getMsgType()); 116 | } catch (IOException ignored) { 117 | // This can happen when px4 shuts down and the connection is dropped. 118 | if (debug) { 119 | System.out.println("got exception: " + ignored); 120 | } 121 | reset(); 122 | } 123 | } 124 | } 125 | 126 | private void IndicateReceivedMessage(int type) { 127 | if (monitorMessage) { 128 | boolean shouldPrint = false; 129 | int count = 0; 130 | // if the list of messages to monitor is empty, but the flag is on, monitor all messages. 131 | if (monitorMessageIDs.isEmpty()) { 132 | if (messageCounts.containsKey(type)) { count = messageCounts.get(type); } 133 | shouldPrint = count >= MONITOR_MESSAGE_RATE; 134 | } else { 135 | // otherwise, only print messages in the list of message IDs we're monitoring. 136 | if (messageCounts.containsKey(type)) { count = messageCounts.get(type); } 137 | shouldPrint = count >= MONITOR_MESSAGE_RATE && monitorMessageIDs.contains(type); 138 | } 139 | printMessage(shouldPrint, count, type); 140 | } 141 | } 142 | 143 | private void printMessage(boolean should, int count, int type) { 144 | if (should) { 145 | System.out.println(type); 146 | messageCounts.put(type, 0); 147 | if (time >= TIME_PASSING) { 148 | System.out.println("---"); 149 | time = 0; 150 | } else { 151 | time++; 152 | } 153 | } else { 154 | messageCounts.put(type, count + 1); 155 | } 156 | } 157 | 158 | @Override 159 | public void update(long t, boolean paused) { 160 | while (isOpened()) { 161 | try { 162 | MAVLinkMessage msg = stream.read(); 163 | if (msg == null) { 164 | break; 165 | } 166 | if (debug) { 167 | System.out.println("[update] msg.name: " + msg.getMsgName() + ", type: " + msg.getMsgType()); 168 | } 169 | IndicateReceivedMessage(msg.getMsgType()); 170 | msg.forwarded = true; 171 | sendMessage(msg); 172 | } catch (IOException ignored) { 173 | // This can happen when px4 shuts down and the connection is dropped. 174 | if (debug) { 175 | System.out.println("Received IOException"); 176 | } 177 | reset(); 178 | return; 179 | } 180 | } 181 | } 182 | } 183 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/Target.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import com.sun.j3d.utils.geometry.Sphere; 4 | import me.drton.jmavlib.geo.GlobalPositionProjector; 5 | import me.drton.jmavlib.geo.LatLonAlt; 6 | 7 | import javax.vecmath.Vector3d; 8 | import java.io.FileNotFoundException; 9 | 10 | /** 11 | * User: ton Date: 01.02.14 Time: 22:12 12 | */ 13 | public abstract class Target extends KinematicObject { 14 | private GlobalPositionProjector gpsProjector = new GlobalPositionProjector(); 15 | 16 | public Target(World world, double size, boolean showGui) throws FileNotFoundException { 17 | super(world, showGui); 18 | Sphere sphere = new Sphere((float) size); 19 | transformGroup.addChild(sphere); 20 | gpsProjector.init(world.getGlobalReference()); 21 | } 22 | 23 | public GNSSReport getGlobalPosition() { 24 | Vector3d pos = getPosition(); 25 | LatLonAlt latLonAlt = gpsProjector.reproject(new double[] {pos.x, pos.y, pos.z}); 26 | GNSSReport gps = new GNSSReport(); 27 | gps.position = latLonAlt; 28 | gps.eph = 1.0f; 29 | gps.epv = 1.0f; 30 | gps.velocity = getVelocity(); 31 | return gps; 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/UDPMavLinkPort.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.mavlink.MAVLinkSchema; 4 | import me.drton.jmavlib.mavlink.MAVLinkStream; 5 | import me.drton.jmavlib.mavlink.MAVLinkMessage; 6 | 7 | import java.io.IOException; 8 | import java.net.*; 9 | import java.nio.ByteBuffer; 10 | import java.nio.channels.DatagramChannel; 11 | import java.util.*; 12 | 13 | /** 14 | * User: ton Date: 02.12.13 Time: 20:56 15 | */ 16 | public class UDPMavLinkPort extends MAVLinkPort { 17 | private MAVLinkSchema schema; 18 | private DatagramChannel channel = null; 19 | private ByteBuffer rxBuffer = ByteBuffer.allocate(8192); 20 | private SocketAddress bindPort = null; 21 | private SocketAddress peerPort; 22 | private MAVLinkStream stream; 23 | private boolean debug = false; 24 | 25 | private boolean monitorMessage = false; 26 | private HashSet monitorMessageIDs; 27 | private HashMap messageCounts = new HashMap(); 28 | 29 | static int MONITOR_MESSAGE_RATE = 100; // rate at which to print message info 30 | static int TIME_PASSING = 10; // change the print so it's visible to the user. 31 | static int time = 0; 32 | 33 | 34 | public UDPMavLinkPort(MAVLinkSchema schema) { 35 | super(schema); 36 | this.schema = schema; 37 | rxBuffer.flip(); 38 | } 39 | 40 | public void setMonitorMessageID(HashSet ids) { 41 | this.monitorMessageIDs = ids; 42 | for (int id : ids) { 43 | messageCounts.put(id, 0); 44 | } 45 | this.monitorMessage = true; 46 | } 47 | 48 | public void setDebug(boolean debug) { 49 | this.debug = debug; 50 | } 51 | 52 | public void setupClient(String peerAddress, int peerPort) throws UnknownHostException, IOException { 53 | this.peerPort = new InetSocketAddress(peerAddress, peerPort); 54 | this.bindPort = new InetSocketAddress("0.0.0.0", 0); 55 | if (debug) { 56 | System.out.println("peerAddress: " + this.peerPort.toString() + ", bindAddress: " + 57 | this.bindPort.toString()); 58 | } 59 | 60 | channel = DatagramChannel.open(); 61 | channel.socket().bind(bindPort); 62 | channel.configureBlocking(false); 63 | channel.connect(this.peerPort); 64 | } 65 | 66 | public void setupHost(int peerPort) throws UnknownHostException, IOException { 67 | this.bindPort = new InetSocketAddress("0.0.0.0", peerPort); 68 | channel = DatagramChannel.open(); 69 | channel.socket().bind(bindPort); 70 | channel.configureBlocking(true); 71 | System.out.println("waiting for first message from: " + this.bindPort.toString()); 72 | this.peerPort = channel.receive(this.rxBuffer); 73 | System.out.println("received first message from: " + this.peerPort.toString()); 74 | channel.configureBlocking(false); 75 | channel.connect(this.peerPort); 76 | } 77 | 78 | public void open() throws IOException { 79 | stream = new MAVLinkStream(schema, channel); 80 | stream.setDebug(debug); 81 | } 82 | 83 | @Override 84 | public void close() throws IOException { 85 | if (channel != null) { 86 | channel.close(); 87 | } 88 | } 89 | 90 | @Override 91 | public boolean isOpened() { 92 | return channel != null && channel.isOpen(); 93 | } 94 | 95 | @Override 96 | public void handleMessage(MAVLinkMessage msg) { 97 | if (debug) { System.out.println("[handleMessage] msg.name: " + msg.getMsgName() + ", type: " + msg.getMsgType()); } 98 | 99 | try { 100 | /*SocketAddress remote =*/ channel.getRemoteAddress(); 101 | } catch (IOException e) { 102 | System.err.println(e.toString()); 103 | } 104 | 105 | 106 | if (isOpened()) { 107 | try { 108 | stream.write(msg); 109 | IndicateReceivedMessage(msg.getMsgType()); 110 | } catch (IOException ignored) { 111 | // Silently ignore this exception, we likely just have nobody on this port yet/already 112 | } 113 | } 114 | } 115 | 116 | private void IndicateReceivedMessage(int type) { 117 | if (monitorMessage) { 118 | boolean shouldPrint = false; 119 | int count = 0; 120 | // if the list of messages to monitor is empty, but the flag is on, monitor all messages. 121 | if (monitorMessageIDs.isEmpty()) { 122 | if (messageCounts.containsKey(type)) { count = messageCounts.get(type); } 123 | shouldPrint = count >= MONITOR_MESSAGE_RATE; 124 | } else { 125 | // otherwise, only print messages in the list of message IDs we're monitoring. 126 | if (messageCounts.containsKey(type)) { count = messageCounts.get(type); } 127 | shouldPrint = count >= MONITOR_MESSAGE_RATE && monitorMessageIDs.contains(type); 128 | } 129 | printMessage(shouldPrint, count, type); 130 | } 131 | } 132 | 133 | private void printMessage(boolean should, int count, int type) { 134 | if (should) { 135 | System.out.println(type); 136 | messageCounts.put(type, 0); 137 | if (time >= TIME_PASSING) { 138 | System.out.println("---"); 139 | time = 0; 140 | } else { 141 | time++; 142 | } 143 | } else { 144 | messageCounts.put(type, count + 1); 145 | } 146 | } 147 | 148 | @Override 149 | public void update(long t, boolean paused) { 150 | while (isOpened()) { 151 | try { 152 | MAVLinkMessage msg = stream.read(); 153 | if (msg == null) { 154 | break; 155 | } 156 | if (debug) { 157 | System.out.println("[update] msg.name: " + msg.getMsgName() + ", type: " + msg.getMsgType()); 158 | } 159 | IndicateReceivedMessage(msg.getMsgType()); 160 | msg.forwarded = true; 161 | sendMessage(msg); 162 | } catch (IOException e) { 163 | // Silently ignore this exception, we likely just have nobody on this port yet/already 164 | return; 165 | } 166 | } 167 | } 168 | } 169 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/World.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | import me.drton.jmavlib.geo.LatLonAlt; 4 | 5 | import java.util.ArrayList; 6 | import java.util.List; 7 | 8 | /** 9 | * User: ton Date: 02.02.14 Time: 11:33 10 | */ 11 | public class World { 12 | private List objects = new ArrayList(); 13 | private Environment environment = null; 14 | private LatLonAlt globalReference = new LatLonAlt(0.0, 0.0, 0.0); 15 | 16 | public void addObject(WorldObject obj) { 17 | objects.add(obj); 18 | if (obj instanceof Environment) { 19 | environment = (Environment) obj; 20 | } 21 | } 22 | 23 | public List getObjects() { 24 | return objects; 25 | } 26 | 27 | public Environment getEnvironment() { 28 | return environment; 29 | } 30 | 31 | public synchronized void update(long t, boolean paused) { 32 | for (WorldObject obj : objects) { 33 | obj.update(t, paused); 34 | } 35 | } 36 | 37 | public void setGlobalReference(LatLonAlt globalReference) { 38 | this.globalReference = globalReference; 39 | } 40 | 41 | public LatLonAlt getGlobalReference() { 42 | return globalReference; 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/WorldObject.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim; 2 | 3 | /** 4 | * User: ton Date: 02.02.14 Time: 11:33 5 | */ 6 | public abstract class WorldObject { 7 | protected final World world; 8 | 9 | public WorldObject(World world) { 10 | this.world = world; 11 | } 12 | 13 | public abstract void update(long t, boolean paused); 14 | 15 | public World getWorld() { 16 | return world; 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/vehicle/AbstractMulticopter.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim.vehicle; 2 | 3 | import me.drton.jmavsim.ReportUtil; 4 | import me.drton.jmavsim.Rotor; 5 | import me.drton.jmavsim.World; 6 | 7 | import javax.vecmath.Vector3d; 8 | 9 | /** 10 | * Abstract multicopter class. Does all necessary calculations for multirotor with any placement of rotors. 11 | * Only rotors on one plane supported now. 12 | */ 13 | public abstract class AbstractMulticopter extends AbstractVehicle { 14 | private double dragMove = 0.0; 15 | private double dragRotate = 0.0; 16 | protected Rotor[] rotors; 17 | 18 | public AbstractMulticopter(World world, String modelName, boolean showGui) { 19 | super(world, modelName, showGui); 20 | rotors = new Rotor[getRotorsNum()]; 21 | for (int i = 0; i < getRotorsNum(); i++) { 22 | rotors[i] = new Rotor(); 23 | } 24 | } 25 | 26 | public void report(StringBuilder builder) { 27 | super.report(builder); 28 | builder.append("MULTICOPTER"); 29 | builder.append(newLine); 30 | builder.append("==========="); 31 | builder.append(newLine); 32 | 33 | for (int i = 0; i < getRotorsNum(); i++) { 34 | reportRotor(builder, i); 35 | builder.append(newLine); 36 | } 37 | 38 | } 39 | 40 | private void reportRotor(StringBuilder builder, int rotorIndex) { 41 | Rotor rotor = rotors[rotorIndex]; 42 | 43 | builder.append("ROTOR #"); 44 | builder.append(rotorIndex); 45 | builder.append(newLine); 46 | 47 | builder.append("Control: "); 48 | builder.append(String.format("%s", ReportUtil.d2str(rotor.getControl()))); 49 | builder.append(newLine); 50 | 51 | builder.append("Thrust: "); 52 | builder.append(String.format("%s", ReportUtil.d2str(rotor.getThrust()))); 53 | builder.append(" / "); 54 | builder.append(String.format("%s", ReportUtil.d2str(rotor.getFullThrust()))); 55 | builder.append(" [N]"); 56 | builder.append(newLine); 57 | 58 | builder.append("Torque: "); 59 | builder.append(String.format("%s", ReportUtil.d2str(rotor.getTorque()))); 60 | builder.append(" / "); 61 | builder.append(String.format("%s", ReportUtil.d2str(rotor.getFullTorque()))); 62 | builder.append(" [Nm]"); 63 | builder.append(newLine); 64 | 65 | builder.append("Spin up: "); 66 | builder.append(String.format("%s", ReportUtil.d2str(rotor.getTimeConstant()))); 67 | builder.append(" [s]"); 68 | builder.append(newLine); 69 | 70 | builder.append("Position: "); 71 | builder.append(ReportUtil.vector2str(getRotorPosition(rotorIndex))); 72 | builder.append(newLine); 73 | 74 | } 75 | 76 | /** 77 | * Get number of rotors. 78 | * 79 | * @return number of rotors 80 | */ 81 | protected abstract int getRotorsNum(); 82 | 83 | /** 84 | * Get rotor position relative to gravity center of vehicle. 85 | * 86 | * @param i rotor number 87 | * @return rotor radius-vector from GC 88 | */ 89 | protected abstract Vector3d getRotorPosition(int i); 90 | 91 | public void setDragMove(double dragMove) { 92 | this.dragMove = dragMove; 93 | } 94 | 95 | public void setDragRotate(double dragRotate) { 96 | this.dragRotate = dragRotate; 97 | } 98 | 99 | @Override 100 | public void update(long t, boolean paused) { 101 | if (paused) { 102 | return; 103 | } 104 | for (Rotor rotor : rotors) { 105 | rotor.update(t, paused); 106 | } 107 | super.update(t, paused); 108 | for (int i = 0; i < rotors.length; i++) { 109 | double c = control.size() > i ? control.get(i) : 0.0; 110 | rotors[i].setControl(c); 111 | } 112 | } 113 | 114 | @Override 115 | protected Vector3d getForce() { 116 | int n = getRotorsNum(); 117 | Vector3d f = new Vector3d(); 118 | for (int i = 0; i < n; i++) { 119 | f.z -= rotors[i].getThrust(); 120 | } 121 | rotation.transform(f); 122 | Vector3d airSpeed = new Vector3d(getVelocity()); 123 | airSpeed.scale(-1.0); 124 | if (!ignoreWind) { 125 | airSpeed.add(getWorld().getEnvironment().getCurrentWind(position)); 126 | } 127 | f.add(getAirFlowForce(airSpeed)); 128 | return f; 129 | } 130 | 131 | @Override 132 | protected Vector3d getTorque() { 133 | int n = getRotorsNum(); 134 | Vector3d torque = new Vector3d(); 135 | Vector3d m = new Vector3d(); 136 | Vector3d t = new Vector3d(); 137 | for (int i = 0; i < n; i++) { 138 | // Roll / pitch 139 | t.z = -rotors[i].getThrust(); 140 | m.cross(getRotorPosition(i), t); 141 | // Yaw 142 | m.z -= rotors[i].getTorque(); 143 | torque.add(m); 144 | } 145 | Vector3d airRotationRate = new Vector3d(rotationRate); 146 | airRotationRate.scale(-1.0); 147 | torque.add(getAirFlowTorque(airRotationRate)); 148 | return torque; 149 | } 150 | 151 | protected Vector3d getAirFlowForce(Vector3d airSpeed) { 152 | Vector3d f = new Vector3d(airSpeed); 153 | f.scale(f.length() * dragMove); 154 | return f; 155 | } 156 | 157 | protected Vector3d getAirFlowTorque(Vector3d airRotationRate) { 158 | Vector3d f = new Vector3d(airRotationRate); 159 | f.scale(f.length() * dragRotate); 160 | return f; 161 | } 162 | } 163 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/vehicle/AbstractVehicle.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim.vehicle; 2 | 3 | import me.drton.jmavlib.geo.LatLonAlt; 4 | import me.drton.jmavsim.DynamicObject; 5 | import me.drton.jmavsim.ReportUtil; 6 | import me.drton.jmavsim.ReportingObject; 7 | import me.drton.jmavsim.Sensors; 8 | import me.drton.jmavsim.World; 9 | 10 | import javax.vecmath.Vector3d; 11 | 12 | import java.util.ArrayList; 13 | import java.util.Collections; 14 | import java.util.List; 15 | 16 | /** 17 | * Abstract vehicle class, should be used for creating vehicle of any type. 18 | * Child class should use member 'control' as control input for actuators. 19 | * 'update()' method of AbstractVehicle must be called from child class implementation if overridden. 20 | */ 21 | public abstract class AbstractVehicle extends DynamicObject implements ReportingObject { 22 | protected List control = Collections.emptyList(); 23 | protected Sensors sensors = null; 24 | 25 | public AbstractVehicle(World world, String modelName, boolean showGui) { 26 | super(world, showGui); 27 | if (showGui) { 28 | modelFromFile(modelName); 29 | } 30 | resetObjectParameters(); 31 | } 32 | 33 | public void report(StringBuilder builder) { 34 | Vector3d tv = new Vector3d(); 35 | builder.append("VEHICLE"); 36 | builder.append(newLine); 37 | builder.append("==========="); 38 | builder.append(newLine); 39 | 40 | builder.append("Pos: "); 41 | builder.append(ReportUtil.vector2str(position)); 42 | // builder.append(String.format(" X: %.5f Y: %.5f Z: %.5f", position.x, position.y, position.z)); 43 | builder.append(newLine); 44 | 45 | builder.append("Vel: "); 46 | builder.append(ReportUtil.vector2str(velocity)); 47 | builder.append(newLine); 48 | 49 | builder.append("Acc: "); 50 | builder.append(ReportUtil.vector2str(acceleration)); 51 | builder.append(newLine); 52 | 53 | builder.append("Rot: "); 54 | builder.append(ReportUtil.vector2str(rotationRate)); 55 | builder.append(newLine); 56 | 57 | builder.append("Att: "); 58 | builder.append(ReportUtil.vector2str(ReportUtil.vectRad2Deg(attitude))); 59 | builder.append(newLine); 60 | builder.append(newLine); 61 | 62 | if (sensors != null) { 63 | builder.append("SENSORS"); 64 | builder.append(newLine); 65 | builder.append("--------"); 66 | builder.append(newLine); 67 | 68 | tv = sensors.getAcc(); 69 | builder.append("ACC: "); 70 | builder.append(ReportUtil.vector2str(tv)); 71 | builder.append(newLine); 72 | builder.append(String.format(" Magnitude: %s;", 73 | ReportUtil.d2str(Math.sqrt(tv.x * tv.x + tv.y * tv.y + tv.z * tv.z)))); 74 | builder.append(newLine); 75 | builder.append(String.format(" P: %s; R: %s", ReportUtil.d2str(Math.toDegrees(Math.atan2(tv.x, 76 | -tv.z))), ReportUtil.d2str(Math.toDegrees(Math.atan2(-tv.y, -tv.z))))); 77 | builder.append(newLine + newLine); 78 | 79 | tv = sensors.getGyro(); 80 | builder.append("GYO: "); 81 | builder.append(ReportUtil.vector2str(tv)); 82 | builder.append(newLine); 83 | builder.append(String.format(" Magnitude: %s;", 84 | ReportUtil.d2str(Math.sqrt(tv.x * tv.x + tv.y * tv.y + tv.z * tv.z)))); 85 | builder.append(newLine + newLine); 86 | 87 | tv = sensors.getMag(); 88 | builder.append("MAG: "); 89 | builder.append(ReportUtil.vector2str(tv)); 90 | builder.append(newLine); 91 | builder.append(String.format(" Magnitude: %s;", 92 | ReportUtil.d2str(Math.sqrt(tv.x * tv.x + tv.y * tv.y + tv.z * tv.z)))); 93 | builder.append(newLine + newLine); 94 | 95 | LatLonAlt pos; 96 | if (sensors.getGNSS() != null && sensors.getGNSS().position != null) { 97 | pos = sensors.getGNSS().position; 98 | } else { 99 | pos = sensors.getGlobalPosition(); 100 | } 101 | builder.append(String.format("GPS Lat: %+013.8f;\n Lon: %+013.8f\n Alt: %07.3f", pos.lat, 102 | pos.lon, pos.alt)); 103 | builder.append(newLine); 104 | builder.append(String.format("Baro Alt: %07.3f; Pa: %08.2f", sensors.getPressureAlt(), 105 | sensors.getPressure())); 106 | builder.append(newLine + newLine); 107 | } 108 | 109 | } 110 | 111 | public void setControl(List control) { 112 | this.control = new ArrayList(control); 113 | } 114 | 115 | public List getControl() { 116 | return control; 117 | } 118 | 119 | /** 120 | * Set sensors object for the vehicle. 121 | * 122 | * @param sensors 123 | */ 124 | public void setSensors(Sensors sensors, long t) { 125 | this.sensors = sensors; 126 | sensors.setObject(this, t); 127 | } 128 | 129 | public Sensors getSensors() { 130 | return sensors; 131 | } 132 | 133 | @Override 134 | public void resetObjectParameters() { 135 | super.resetObjectParameters(); 136 | position.set(0.0, 0.0, 0.0); 137 | if (sensors != null) { 138 | sensors.setReset(true); 139 | } 140 | } 141 | 142 | @Override 143 | public void update(long t, boolean paused) { 144 | if (paused) { 145 | return; 146 | } 147 | super.update(t, paused); 148 | if (sensors != null) { 149 | sensors.update(t, paused); 150 | } 151 | } 152 | } 153 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/vehicle/Hexacopter.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim.vehicle; 2 | 3 | import me.drton.jmavsim.Rotor; 4 | import me.drton.jmavsim.World; 5 | 6 | import javax.vecmath.Matrix3d; 7 | import javax.vecmath.Vector3d; 8 | 9 | /** 10 | * Generic hexacopter model. 11 | */ 12 | public class Hexacopter extends AbstractMulticopter { 13 | private static final int rotorsNum = 6; 14 | private Vector3d[] rotorPositions = new Vector3d[rotorsNum]; 15 | 16 | /** 17 | * Generic hexacopter constructor. 18 | * 19 | * @param world world where to place the vehicle 20 | * @param modelName filename of model to load, in .obj format 21 | * @param orientation "x" or "+" 22 | * @param armLength length of arm from center 23 | * @param rotorThrust full thrust of one rotor 24 | * @param rotorTorque torque at full thrust of one rotor 25 | * @param rotorTimeConst spin-up time of rotor 26 | * @param rotorsOffset rotors positions offset from gravity center 27 | * @param showGui false if the GUI has been disabled 28 | */ 29 | public Hexacopter(World world, String modelName, String orientation, double armLength, 30 | double rotorThrust, double rotorTorque, double rotorTimeConst, 31 | Vector3d rotorsOffset, boolean showGui) { 32 | super(world, modelName, showGui); 33 | rotorPositions[0] = new Vector3d(armLength, 0.0, 0.0); 34 | rotorPositions[1] = new Vector3d(-armLength, 0.0, 0.0); 35 | rotorPositions[2] = new Vector3d(-armLength * Math.cos(Math.PI / 3), 36 | -armLength * Math.sin(Math.PI / 3), 0.0); 37 | rotorPositions[3] = new Vector3d(armLength * Math.cos(Math.PI / 3), 38 | armLength * Math.sin(Math.PI / 3), 0.0); 39 | rotorPositions[4] = new Vector3d(armLength * Math.cos(Math.PI / 3), 40 | -armLength * Math.sin(Math.PI / 3), 0.0); 41 | rotorPositions[5] = new Vector3d(-armLength * Math.cos(Math.PI / 3), 42 | armLength * Math.sin(Math.PI / 3), 0.0); 43 | if (orientation.equals("x") || orientation.equals("X")) { 44 | Matrix3d r = new Matrix3d(); 45 | r.rotZ(Math.PI / 2); 46 | for (int i = 0; i < rotorsNum; i++) { 47 | r.transform(rotorPositions[i]); 48 | } 49 | } else if (orientation.equals("+")) { 50 | } else { 51 | throw new RuntimeException("Unknown orientation: " + orientation); 52 | } 53 | for (int i = 0; i < rotors.length; i++) { 54 | rotorPositions[i].add(rotorsOffset); 55 | Rotor rotor = rotors[i]; 56 | rotor.setFullThrust(rotorThrust); 57 | rotor.setFullTorque((i == 1 || i == 3 || i == 4) ? -rotorTorque : rotorTorque); 58 | rotor.setTimeConstant(rotorTimeConst); 59 | } 60 | } 61 | 62 | @Override 63 | protected int getRotorsNum() { 64 | return rotorsNum; 65 | } 66 | 67 | @Override 68 | protected Vector3d getRotorPosition(int i) { 69 | return rotorPositions[i]; 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /src/me/drton/jmavsim/vehicle/Quadcopter.java: -------------------------------------------------------------------------------- 1 | package me.drton.jmavsim.vehicle; 2 | 3 | import me.drton.jmavsim.Rotor; 4 | import me.drton.jmavsim.World; 5 | 6 | import javax.vecmath.Matrix3d; 7 | import javax.vecmath.Vector3d; 8 | 9 | /** 10 | * Generic quadcopter model. 11 | */ 12 | public class Quadcopter extends AbstractMulticopter { 13 | private static final int rotorsNum = 4; 14 | private Vector3d[] rotorPositions = new Vector3d[rotorsNum]; 15 | private int[] rotorRotations = new int[rotorsNum]; 16 | 17 | /** 18 | * Generic quadcopter constructor. 19 | * 20 | * @param world world where to place the vehicle 21 | * @param modelName filename of model to load, in .obj format 22 | * @param orientation "x" or "+" 23 | * @param style rotor position layout style. "default"/"px4" for px4, or "cw_fr" CW sequential layout starting at front motor 24 | * @param armLength length of arm from center [m] 25 | * @param rotorThrust full thrust of one rotor [N] 26 | * @param rotorTorque torque at full thrust of one rotor in [Nm] 27 | * @param rotorTimeConst spin-up time of rotor [s] 28 | * @param rotorsOffset rotors positions offset from gravity center 29 | * @param showGui false if the GUI has been disabled 30 | */ 31 | public Quadcopter(World world, String modelName, String orientation, String style, 32 | double armLength, double rotorThrust, double rotorTorque, 33 | double rotorTimeConst, Vector3d rotorsOffset, boolean showGui) { 34 | super(world, modelName, showGui); 35 | 36 | int i; 37 | 38 | if (style == "cw_fr") { 39 | rotorPositions[0] = new Vector3d(armLength, 0.0, 0.0); 40 | rotorPositions[1] = new Vector3d(0.0, armLength, 0.0); 41 | rotorPositions[2] = new Vector3d(-armLength, 0.0, 0.0); 42 | rotorPositions[3] = new Vector3d(0.0, -armLength, 0.0); 43 | for (i = 0; i < rotorsNum; ++i) { 44 | rotorRotations[i] = ((i % 2) == 0) ? 1 : -1; 45 | } 46 | } else { 47 | rotorPositions[0] = new Vector3d(0.0, armLength, 0.0); 48 | rotorPositions[1] = new Vector3d(0.0, -armLength, 0.0); 49 | rotorPositions[2] = new Vector3d(armLength, 0.0, 0.0); 50 | rotorPositions[3] = new Vector3d(-armLength, 0.0, 0.0); 51 | for (i = 0; i < rotorsNum; ++i) { 52 | rotorRotations[i] = (i < 2) ? -1 : 1; 53 | } 54 | } 55 | 56 | if (orientation.equals("x") || orientation.equals("X")) { 57 | Matrix3d r = new Matrix3d(); 58 | r.rotZ(-Math.PI / 4); 59 | for (i = 0; i < rotorsNum; i++) { 60 | r.transform(rotorPositions[i]); 61 | } 62 | } else if (!orientation.equals("+")) { 63 | throw new RuntimeException("Unknown orientation: " + orientation); 64 | } 65 | for (i = 0; i < rotors.length; i++) { 66 | rotorPositions[i].add(rotorsOffset); 67 | Rotor rotor = rotors[i]; 68 | rotor.setFullThrust(rotorThrust); 69 | rotor.setFullTorque(rotorTorque * rotorRotations[i]); 70 | rotor.setTimeConstant(rotorTimeConst); 71 | } 72 | } 73 | 74 | @Override 75 | protected int getRotorsNum() { 76 | return rotorsNum; 77 | } 78 | 79 | @Override 80 | protected Vector3d getRotorPosition(int i) { 81 | return rotorPositions[i]; 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /tools/RC2Serial/RC2Serial.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * RC2Serial: Simple PPM -> serial converter on Arduino. 3 | * 4 | * Wiring: 5 | * Pin 3: PPM input 6 | * Output send via USB 7 | * 8 | * Serial output on USB: 115200, 8 data bits, 1 stop bit, no parity 9 | * Frame contains 2 byte header ("RC") and 16 channels values multiplied by 10 (e.g. 15000 for 1500 us), in uint16_t (big endian): 10 | * [uint8_t header0] [uint8_t header0] [uint16_t channel0] [uint16_t channel1] ... [uint16_t channel15] 11 | */ 12 | 13 | #define PPM_Pin 3 // this must be 2 or 3 14 | #define MULTIPLIER (F_CPU / 1000000 / 8) // TIMER1 prescaler: 8 15 | #define PPM_CHANNELS_MAX 16 16 | 17 | #pragma push(pack, 1) 18 | struct { 19 | uint8_t header0; 20 | uint8_t header1; 21 | uint16_t values[PPM_CHANNELS_MAX]; 22 | } data; 23 | #pragma pop(pack) 24 | 25 | void setup() { 26 | Serial.begin(115200); 27 | 28 | pinMode(PPM_Pin, INPUT); 29 | attachInterrupt(PPM_Pin - 2, read_ppm, FALLING); 30 | 31 | TCCR1A = 0; // reset TIMER1 32 | TCCR1B = 0; 33 | TCCR1B |= (1 << CS11); // set TIMER1 prescaler: 8 34 | } 35 | 36 | void loop() { 37 | delay(1000); 38 | } 39 | 40 | void read_ppm() { 41 | static byte channel = 0; 42 | 43 | unsigned long counter = TCNT1; 44 | TCNT1 = 0; 45 | 46 | if (counter > 2500 * MULTIPLIER) { 47 | data.header0 = 'R'; 48 | data.header1 = 'C'; 49 | for (int i = channel; i < PPM_CHANNELS_MAX; i++) { 50 | data.values[i] = 0; 51 | } 52 | channel = 0; 53 | Serial.write((byte*)&data, sizeof(data)); 54 | } else { 55 | if (channel < PPM_CHANNELS_MAX) { 56 | uint16_t v = counter * 10 / MULTIPLIER; 57 | data.values[channel++] = (v >> 8 | v << 8); // Swap bytes to big endian 58 | } 59 | } 60 | } 61 | --------------------------------------------------------------------------------