├── .gitignore ├── LICENSE ├── README.md ├── cognifly ├── .gitignore ├── __init__.py ├── __main__.py ├── cognifly_controller │ ├── __init__.py │ ├── cognifly_controller.py │ ├── dummy_controller.py │ └── trace_logs │ │ └── .gitignore ├── cognifly_remote │ ├── __init__.py │ ├── cognifly_remote.py │ └── sprites │ │ ├── gamepad_off.png │ │ └── gamepad_on.png └── utils │ ├── __init__.py │ ├── filters.py │ ├── functions.py │ ├── ip_tools.py │ ├── joysticks.py │ ├── pid.py │ ├── tcp_video_interface.py │ └── udp_interface.py ├── old_controller.py ├── readme ├── DRONE_SETUP.md └── figures │ ├── gui.PNG │ └── ps4.PNG ├── scripts └── cognifly-controller ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | *.ipynb 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | .python-version 87 | 88 | # pipenv 89 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 90 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 91 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 92 | # install all needed dependencies. 93 | #Pipfile.lock 94 | 95 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 96 | __pypackages__/ 97 | 98 | # Celery stuff 99 | celerybeat-schedule 100 | celerybeat.pid 101 | 102 | # SageMath parsed files 103 | *.sage.py 104 | 105 | # Environments 106 | .env 107 | .venv 108 | env/ 109 | venv/ 110 | ENV/ 111 | env.bak/ 112 | venv.bak/ 113 | 114 | # Spyder project settings 115 | .spyderproject 116 | .spyproject 117 | 118 | # Rope project settings 119 | .ropeproject 120 | 121 | # mkdocs documentation 122 | /site 123 | 124 | # mypy 125 | .mypy_cache/ 126 | .dmypy.json 127 | dmypy.json 128 | 129 | # Pyre type checker 130 | .pyre/ 131 | 132 | .idea/ 133 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 The CogniFly Project 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # cognifly-python 2 | Control the CogniFly open-source drone remotely from your python script. 3 | 4 | ## Quick links 5 | 6 | - [Drone setup instructions](/readme/DRONE_SETUP.md) 7 | - [Installation](#installation) 8 | - [Usage](#usage) 9 | - [Manual control](#manual-control) 10 | - [Python API](#control-though-the-python-api) 11 | - [Pro API](#pro-api) 12 | - [School API](#school-api) 13 | - [Streaming](#streaming) 14 | - [Troubleshooting](#troubleshooting) 15 | - [Advanced](#advanced-usage) 16 | - [Custom estimator and PIDs](#custom-estimator-and-pids) 17 | 18 | 19 | ## Installation 20 | 21 | Please follow [drone setup instructions](/readme/DRONE_SETUP.md) to properly set up the flight controller and raspberry pi. 22 | 23 | `cognifly-python` can be installed from PyPI. 24 | This is done by simply executing the following on both the Raspberry Pi and the remote-controlling computer: 25 | 26 | ```bash 27 | pip3 install --upgrade pip 28 | pip3 install cognifly 29 | ``` 30 | 31 | ## Usage 32 | 33 | In order to use the installed library, the `cognifly_controller.py` script must first be running on Cognifly. 34 | This can be done by connecting to the drone through SSH, and executing the following command: 35 | 36 | ```bash 37 | cognifly-controller 38 | ``` 39 | 40 | We advise setting up a service on the rapsberry pi to launch this script automatically on CogniFly at startup, so that the user doesn't need to SSH the drone. 41 | 42 | _Note: if using our Raspbian image on the Raspberry Pi, this service is set up already._ 43 | 44 | *Note: On the Raspberry Pi, the `cognifly-controller` command may not become available immediately after installation. If not, try closing the terminal and opening a new one. 45 | Worst case scenario, this command is an alias for `python3 -m cognifly controller`.* 46 | 47 | 48 | ### Manual control 49 | 50 | #### PS4 controller: 51 | 52 | We recommend using a PS4 bluetooth gamepad for manual control of the drone, as this is pretty fun and allows you to use the drone everywhere. 53 | 54 | First, pair your PS4 controller with the Raspberry Pi (you need to do this only once): 55 | - ssh the Raspberry Pi: `ssh pi@my_drone_hostname.local` 56 | - install the prerequisite: `sudo apt install bluetooth pi-bluetooth bluez` 57 | - reboot the pi: `sudo reboot` 58 | - ssh again: `ssh pi@my_drone_hostname.local` 59 | - execute `sudo bluetoothctl` 60 | - configure the bluetooth agent by executing the following commands: 61 | ```terminal 62 | agent on 63 | discoverable on 64 | pairable on 65 | default-agent 66 | ``` 67 | - launch the scan for available bluetooth devices: `scan on` 68 | - start bluetooth pairing on the PS4 controller by pressing `share`, then the `PS` buttons simultaneously until a white light flashes 69 | - in the scan output, look for something like `[NEW] Device AC:FD:93:14:25:D3 Wireless Controller` 70 | - copy the MAC address (`AC:FD:93:14:25:D3` in this example but yours will differ) 71 | - turn off scanning: `scan off` 72 | - pair: `pair XX:XX:XX:XX:XX:XX` by replacing `XX:XX:XX:XX:XX:XX` with your copied MAC address 73 | - connect to the controller: `connect XX:XX:XX:XX:XX:XX` (replacing `XX:XX:XX:XX:XX:XX` with your copied MAC address) 74 | - the light stops flashing and turns blue 75 | - trust the controller: `trust XX:XX:XX:XX:XX:XX` (replacing `XX:XX:XX:XX:XX:XX` with your copied MAC address) 76 | 77 | Once this is done, you will not need to ssh the drone anymore; just turn your PS4 controller on and it will connect to the Raspberry Pi automatically (it may sometimes take several trials). 78 | 79 | Controls: 80 | 81 | ![ps4](readme/figures/ps4.PNG) 82 | 83 | _Note: when taking control over the API, the flight controller of the API is disabled but the `DISARM` emergency button and the video stream will still work. 84 | When releasing control to the API, the `DISARM` buttons of the PS4 controller will also keep working. 85 | By default, when a PS4 controller is connected, it takes control over the API. 86 | The `Gamepad` indicator in the GUI monitors this._ 87 | 88 | #### Keyboard: 89 | 90 | It is possible to manually control the drone with the keyboard via SSH, by focusing the session that executes `cognifly_controller.py` on the raspberry pi: 91 | 92 | - `A`: arm 93 | - `D`: disarm 94 | - `T`: take off 95 | - `L`: land 96 | - `8`: forward 97 | - `5`: backward 98 | - `7`: left 99 | - `9`: right 100 | - `4`: left yaw 101 | - `6`: right yaw 102 | - `pageup`: up 103 | - `pagedown`: down 104 | - `R`: reset the board and exit the script 105 | 106 | ### Control though the python API 107 | 108 | The remote control API is defined in [cognifly_remote.py](https://github.com/thecognifly/cognifly-python/blob/main/cognifly/cognifly_remote/cognifly_remote.py) (please read the docstrings for thorough documentation). 109 | 110 | Connecting to the drone is as simple as creating a `Cognifly` object. 111 | By default, this will also pop a simple Graphic User Interface: 112 | 113 | ```python 114 | from cognifly import Cognifly 115 | 116 | # connect to the drone and pop the GUI: 117 | cf = Cognifly(drone_hostname="my_drone_name.local") 118 | 119 | time.sleep(10.0) 120 | ``` 121 | 122 | ![gui](readme/figures/gui.PNG) 123 | 124 | The API is divided into a "pro" and a "school" API. 125 | 126 | #### Pro API 127 | 128 | The "pro" API is fairly simple and is what you should use for serious applications. 129 | It enables the user to control Cognifly either by velocity or by position, in two possible coordinate systems: 130 | - world frame: X and Y relative to the starting point of the drone, Yaw relative to the starting orientation of the drone, and Z relative to the ground, 131 | - drone frame: X, Y and Yaw relative to the current position and orientation of the drone, and Z relative to the ground. 132 | 133 | Example-script using the "pro" API for control: 134 | 135 | ```python 136 | import time 137 | from cognifly import Cognifly 138 | 139 | # create a Cognifly object (resets the controller): 140 | cf = Cognifly(drone_hostname="my_drone_name.local") 141 | 142 | # arm the drone: 143 | cf.arm() 144 | time.sleep(1.0) 145 | 146 | # take off to default altitude : 147 | cf.takeoff_nonblocking() 148 | time.sleep(10.0) 149 | 150 | # go 0.2 m/s frontward for 1.0 second: 151 | cf.set_velocity_nonblocking(v_x=0.2, v_y=0.0, v_z=0.0, w=0.0, duration=1.0, drone_frame=True) 152 | time.sleep(2.0) 153 | 154 | # go rightward and upward while rotating for 1.0 second: 155 | cf.set_velocity_nonblocking(v_x=0.0, v_y=0.2, v_z=0.1, w=0.5, duration=1.0, drone_frame=True) 156 | time.sleep(2.0) 157 | 158 | # retrieve battery, pose, speed and health flags: 159 | telemetry = cf.get_telemetry() 160 | print(f"telemetry:\n{telemetry}") 161 | 162 | # go to (-0.5, -0.5, 0.5) and back to the initial yaw (0.0) at a max speed of 0.25 m/s, 0.5 rad/s: 163 | cf.set_position_nonblocking(x=-0.5, y=-0.5, z=0.5, yaw=0.0, 164 | max_velocity=0.25, max_yaw_rate=0.5, max_duration=10.0, relative=False) 165 | time.sleep(5.0) 166 | 167 | # go frontward for 0.5 m at 0.25 m/s, staying at an altitude of 0.5 m: 168 | cf.set_position_nonblocking(x=0.5, y=0.0, z=0.5, yaw=0.0, 169 | max_velocity=0.25, max_yaw_rate=0.5, max_duration=10.0, relative=True) 170 | time.sleep(5.0) 171 | 172 | # go back home: 173 | cf.set_position_nonblocking(x=0.0, y=0.0, z=0.5, yaw=0.0, 174 | max_velocity=0.25, max_yaw_rate=0.5, max_duration=10.0, relative=False) 175 | time.sleep(5.0) 176 | 177 | # land: 178 | cf.land_nonblocking() 179 | time.sleep(2.0) 180 | 181 | # disarm the drone 182 | cf.disarm() 183 | 184 | # reset the drone (reinitializes coordinate system): 185 | cf.reset() 186 | ``` 187 | 188 | Note that this API is non-blocking, a new call will override the previous call 189 | (position and velocity are controlled by PIDs, so violent changes WILL make the drone unstable). 190 | 191 | #### School API 192 | 193 | The "school" API is an overlay of the "pro" API, built for students who need an easy and relatively safe API for class purpose. 194 | It is vastly inspired from the `easytello` library, of which it reproduces most of the interface, adapted to Cognifly. 195 | Contrary to the "pro" API, calls to the "school" API are blocking and return only when the command has been fully performed (or when it times out). 196 | It mostly consists of hidden calls to the position control "pro" API with an additional hidden callback through the `sleep_until_done` method (see the code to reproduce similar behavior with the "pro" API). 197 | 198 | Whereas the units of the "pro" API are meters and radians, the units of the "school" API are centimeters and degrees. 199 | 200 | Example using the "school" API for control: 201 | ```python 202 | from cognifly import Cognifly 203 | 204 | # create a Cognifly object (resets the controller): 205 | cf = Cognifly(drone_hostname="my_drone_name.local") 206 | 207 | # take off (resets the controller): 208 | cf.takeoff() 209 | 210 | # go forward for 50 cm: 211 | cf.forward(50) 212 | 213 | # turn clockwise by 90 degrees: 214 | cf.cw(90) 215 | 216 | # turn counter-clockwise by 45 degrees: 217 | cf.ccw(45) 218 | 219 | # other movements: 220 | cf.backward(20) 221 | cf.up(30) 222 | cf.down(20) 223 | cf.right(20) 224 | cf.left(10) 225 | 226 | # go to (0, 0, 0.5) (cm) with a yaw of 90° counter-clockwise compared to the initial orientation 227 | cf.go(0, 0, 0.5, -90) 228 | 229 | # sequence of position targets (when 4 items, the last is yaw): 230 | cf.position_sequence([[0.2, 0.2, 0.5, 0.0], 231 | [0.0, 0.1, 0.5], 232 | [0.0, 0.0, 0.5]]) 233 | 234 | # get telemetry: 235 | battery = cf.get_battery() 236 | height = cf.get_height() 237 | speed = cf.get_speed() 238 | x, y, z = cf.get_position() 239 | vx, vy, vz = cf.get_velocity() 240 | 241 | # land (disarms the drone): 242 | cf.land() 243 | ``` 244 | 245 | #### Streaming 246 | 247 | Cognifly can stream from the raspberry pi camera (note: frames are transferred directly through the local network). 248 | First make sure that the camera is enabled in the raspberry pi, and that it works correctly. 249 | 250 | It is possible to display the video or to retrieve frames for processing: 251 | 252 | ```python 253 | from cognifly import Cognifly 254 | 255 | # create a Cognifly object (resets the controller): 256 | cf = Cognifly(drone_hostname="my_drone_name.local") 257 | 258 | # take off (resets the controller): 259 | cf.takeoff() 260 | 261 | # display the stream at 24 fps: 262 | cf.stream(fps=24) 263 | time.sleep(10.0) 264 | 265 | # stop the stream: 266 | cf.streamoff() 267 | time.sleep(5.0) 268 | 269 | # turn the stream on at 5 fps, with no display: 270 | cf.streamon(fps=5) 271 | 272 | # retrieve a frame for processing: 273 | cv2_image = cf.get_frame() 274 | 275 | # turn the stream off: 276 | cf.streamoff() 277 | 278 | # land: 279 | cf.land() 280 | ``` 281 | 282 | ## Troubleshooting 283 | **Drift**: A slight horizontal drift of less than 1cm/s is to be expected. 284 | However, if the drone drifts badly, disarm it, move it around and check that the position and velocity estimates make sense. 285 | - If some estimates remain fixed: the drone is probably not in EST_POS debug mode. Carefully setup the flight controller again, according to the [drone setup instructions](/readme/DRONE_SETUP.md). 286 | - If some estimates behave crazily: the floor is probably not textured enough. The current iteration of CogniFly uses a cheap optical flow sensor to estimate its location, and this sensor needs a lot of texture on the ground to work properly. See the [custom estimator](#custom-estimator) section to circumvent this issue. 287 | 288 | 289 | ## Advanced usage 290 | 291 | ### Custom estimator and PIDs 292 | `cognifly-python` supports custom estimators. 293 | A custom estimator overrides the position and velocity estimates that come from the flight controller, as these can be very poor when the ground is textureless or badly lit, due to the optical flow sensor performing poorly. 294 | 295 | In order to design and use your own custom estimator, you must not use the `cognifly-controller` bash command, but instead write a python script in which you instantiate a `CogniflyController` object, passing a custom `PoseEstimator` to the `pose_estimator` argument. 296 | Doing this also enables customizing the PID values without resorting to the API. 297 | 298 | This can be achieved as follows: 299 | 300 | ```python 301 | # Script to be executed on the drone, instead of the cognifly-controller bash command 302 | 303 | from cognifly.cognifly_controller.cognifly_controller import CogniflyController, PoseEstimator 304 | 305 | # Definition of a custom estimator: 306 | class MyCustomEstimator(PoseEstimator): 307 | 308 | def get(self): 309 | """ 310 | Must return a tuple of 8 values: 311 | (pos_x_wf, pos_y_wf, pos_z_wf, yaw, vel_x_wf, vel_y_wf, vel_z_wf, yaw_rate) 312 | If any is None, this is considered failure and the onboard estimator will be used instead. 313 | 314 | These values represent the drone attitude in the world frame: 315 | pos_x_wf = x position (m) 316 | pos_y_wf = y position (m) 317 | pos_z_wf = z position (m) 318 | yaw: yaw (rad) 319 | vel_x_wf = x velocity (m/s) 320 | vel_y_wf = y velocity (m/s) 321 | vel_z_wf = z velocity (m/s) 322 | yaw_rate: yaw rate (rad/s) 323 | 324 | CAUTION: your estimator needs to respect the cognifly coordinate system, which is not standard: 325 | x: FORWARD 326 | y: RIGHT 327 | z: UP 328 | """ 329 | 330 | # compute pos_x_wf, pos_y_wf, pos_z_wf, yaw, vel_x_wf, vel_y_wf, vel_z_wf, yaw_rate here 331 | 332 | return pos_x_wf, pos_y_wf, pos_z_wf, yaw, vel_x_wf, vel_y_wf, vel_z_wf, yaw_rate 333 | 334 | if __name__ == '__main__': 335 | # instantiate of the custom estimator: 336 | ce = MyCustomEstimator() 337 | 338 | # instantiate of the CogniflyController object: 339 | cc = CogniflyController(print_screen=True, # set to false if you want to run headless 340 | pose_estimator=ce, # your custom estimator 341 | trace_logs=False, # set to True to save PID logs 342 | vel_x_kp=750.0, # proportional gain for X velocity 343 | vel_x_ki=200.0, # integral gain for X velocity 344 | vel_x_kd=10.0, # derivative gain for X velocity 345 | vel_y_kp=750.0, # proportional gain for Y velocity 346 | vel_y_ki=200.0, # integral gain for Y velocity 347 | vel_y_kd=10.0, # derivative gain for Y velocity 348 | vel_z_kp=5.0, # proportional gain for Z velocity 349 | vel_z_ki=2.0, # integral gain for Z velocity 350 | vel_z_kd=0.05, # derivative gain for Z velocity 351 | vel_w_kp=75.0, # proportional gain for yaw rate 352 | vel_w_ki=50.0, # integral gain for yaw rate 353 | vel_w_kd=0.0, # derivative gain for yaw rate 354 | pid_limit=400, # PID clipping value, should not exceed 500 355 | x_vel_gain=0.5, # gain from X vector to X velocity (position control) 356 | y_vel_gain=0.5, # gain from Y vector to Y velocity (position control) 357 | z_vel_gain=0.2) # gain from Z vector to Z velocity (position control) 358 | 359 | # run the controller: 360 | cc.run_curses() 361 | ``` 362 | In case you need the estimates of the flight controller in your custom estimator (e.g., to merge them with your own estimates in a Kalman filter), the `PoseEstimator` class has a `get_fc_estimate()` method for that purpose: 363 | ```python 364 | def get(self): 365 | 366 | pos_x_wf, pos_y_wf, pos_z_wf, yaw, vel_x_wf, vel_y_wf, vel_z_wf, yaw_rate = self.get_fc_estimate() 367 | 368 | # do your stuff here 369 | # (note that the fc estimates will be None before run_curses()) 370 | 371 | return pos_x_wf, pos_y_wf, pos_z_wf, yaw, vel_x_wf, vel_y_wf, vel_z_wf, yaw_rate 372 | ``` 373 | 374 | _Note 1: `cognifly-python` does not entirely override the flight controller estimates internally. 375 | Instead, it consists of an external control loop. 376 | The internal control loop performed within the flight controller is not altered, and still uses the flight controller estimates. 377 | Thus, the drone behavior may still differ depending on the texture of the ground._ 378 | 379 | _Note 2: `mode 2` of the gamepad uses your custom estimator, whereas `mode 1` shuts down the external control loop. 380 | You can use `mode 2` to debug your estimator. 381 | The `trace_logs` argument can also be set to `True` for tuning your PIDs. 382 | Logs will then be saved under `cognifly/cognifly_controller/trace_logs`._ 383 | 384 | :warning: _**Caution**: taking off with the gamepad in `mode 2` momentarily shuts down the external control loop. 385 | However, you can take off using your estimator thanks to the API, using `takeoff_nonblocking(track_xy=True)`._ 386 | -------------------------------------------------------------------------------- /cognifly/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /cognifly/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python API and bluetooth controller for the CogniFly drone. 3 | 4 | Install with pip: pip install cognifly 5 | """ 6 | 7 | __author__ = "Yann Bouteiller, Ricardo de Azambuja" 8 | __copyright__ = "Copyright 2022, MISTLab.ca" 9 | __credits__ = [""] 10 | __license__ = "MIT" 11 | __version__ = "0.2.0" 12 | __maintainer__ = "Yann Bouteiller" 13 | __email__ = "" 14 | __status__ = "Development" 15 | 16 | 17 | from cognifly.cognifly_remote.cognifly_remote import Cognifly 18 | import logging 19 | 20 | 21 | logfilename = "cognifly.log" 22 | logfilemode = 'a' 23 | loglevel = 'INFO' 24 | 25 | logging.basicConfig(format="[%(levelname)s] [%(asctime)s]: %(message)s", 26 | filename=logfilename, 27 | filemode=logfilemode, 28 | level=getattr(logging, loglevel.upper())) 29 | -------------------------------------------------------------------------------- /cognifly/__main__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Usage: 3 | python3 -m cognifly controller 4 | or 5 | python3 -m cognifly controller-headless 6 | """ 7 | 8 | import sys 9 | from cognifly.cognifly_controller.cognifly_controller import run_controller 10 | 11 | _, cmd, *args = sys.argv 12 | trace_logs = "trace" in args or "--trace" in args 13 | 14 | if cmd == "controller": 15 | run_controller(trace_logs=trace_logs) 16 | elif cmd == "controller-headless": 17 | run_controller(print_screen=False, trace_logs=trace_logs) 18 | else: 19 | raise AttributeError("Undefined command: " + cmd) 20 | -------------------------------------------------------------------------------- /cognifly/cognifly_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/cognifly/cognifly_controller/__init__.py -------------------------------------------------------------------------------- /cognifly/cognifly_controller/dummy_controller.py: -------------------------------------------------------------------------------- 1 | import time 2 | from collections import deque 3 | from itertools import cycle 4 | import socket 5 | import pickle as pkl 6 | import numpy as np 7 | from pathlib import Path 8 | 9 | from cognifly.utils.udp_interface import UDPInterface 10 | from cognifly.utils.ip_tools import extract_ip 11 | from cognifly.utils.pid import PID 12 | from cognifly.utils.filters import Simple1DKalman, Simple1DExponentialAverage 13 | from cognifly.utils.functions import clip, smallest_angle_diff_rad 14 | 15 | 16 | KEY_TIMEOUT = 0.2 # seconds before a keyboard command times out 17 | 18 | DEFAULT_ROLL = 1500 19 | DEFAULT_PITCH = 1500 20 | DEFAULT_THROTTLE = 900 # throttle bellow a certain value disarms the FC 21 | DEFAULT_YAW = 1500 22 | 23 | TAKEOFF = 1300 24 | LAND = 900 25 | 26 | MIN_CMD_ROLL = 1000 27 | MIN_CMD_PITCH = 1000 28 | MIN_CMD_THROTTLE = 900 # throttle bellow a certain value disarms the FC 29 | MIN_CMD_YAW = 1400 30 | MAX_CMD_ROLL = 2000 31 | MAX_CMD_PITCH = 2000 32 | MAX_CMD_THROTTLE = 1500 33 | MAX_CMD_YAW = 1600 34 | 35 | KEY_N_ROLL = 1350 36 | KEY_N_PITCH = 1350 37 | KEY_N_THROTTLE = -10 # added on throttle(-) key press 38 | KEY_N_YAW = 1450 39 | KEY_P_ROLL = 1650 40 | KEY_P_PITCH = 1650 41 | KEY_P_THROTTLE = 10 # added on throttle(+) key press 42 | KEY_P_YAW = 1550 43 | 44 | KEY_TAKEOFF = TAKEOFF 45 | KEY_LAND = LAND 46 | 47 | # DEFAULT_CH5 = 1000 # DISARMED (1000) / ARMED (1800) 48 | # DEFAULT_CH6 = 1500 # ANGLE (1000) / NAV_POSHOLD (1500) <= 1300: NAV ALTHOLD 49 | # DEFAULT_CH7 = 1000 # FAILSAFE (1800) 50 | # DEFAULT_CH8 = 1000 # HEADING HOLD (1800) 51 | 52 | ARMED = 1800 53 | DISARMED = 1000 54 | ANGLE_MODE = 1000 55 | NAV_ALTHOLD_MODE = 1500 56 | NAV_POSHOLD_MODE = 1800 57 | 58 | DEFAULT_AUX1 = DISARMED # DISARMED (1000) / ARMED (1800) 59 | DEFAULT_AUX2 = NAV_POSHOLD_MODE # Angle (1000) / NAV_ALTHOLD (1500) / NAV_POSHOLD (1800) 60 | 61 | # Max periods for: 62 | CTRL_LOOP_TIME = 1 / 100 63 | SLOW_MSGS_LOOP_TIME = 1 / 5 # these messages take a lot of time slowing down the loop... 64 | BATT_UDP_MSG_TIME = 1.0 65 | 66 | NO_OF_CYCLES_AVERAGE_GUI_TIME = 10 67 | 68 | BATT_NO_INFO = -1 69 | BATT_OK = 0 70 | BATT_TOO_HIGH = 1 71 | BATT_WARNING = 2 72 | BATT_TOO_LOW = 3 73 | 74 | EPSILON_DIST_TO_TARGET = 0.05 # (m) 75 | EPSILON_ANGLE_TO_TARGET = 1 * np.pi / 180.0 # (rad) 76 | 77 | 78 | class SignalTracer: 79 | """ 80 | This can be used to trace the drone dynamics in real-time (e.g. to output a CSV-like log file) 81 | """ 82 | def __init__(self, filepath): 83 | self.file = open(filepath, 'w+') 84 | 85 | def write_line(self, line): 86 | self.file.write(line + '\n') 87 | 88 | def __del__(self): 89 | self.file.close() 90 | 91 | 92 | class CogniflyControllerDummy: 93 | def __init__(self, 94 | network=True, 95 | drone_hostname=None, 96 | drone_port=8988, 97 | print_screen=True, 98 | obs_loop_time=0.1, 99 | trace_logs=False): 100 | self.network = network 101 | if not self.network: 102 | self.udp_int = None 103 | self.drone_hostname = None 104 | self.drone_port = None 105 | else: 106 | self.udp_int = UDPInterface() 107 | self.drone_hostname = socket.gethostname() if drone_hostname is None else drone_hostname 108 | self.drone_ip = socket.gethostbyname(self.drone_hostname) if drone_hostname is not None else extract_ip() 109 | self.drone_port = drone_port 110 | self.udp_int.init_receiver(ip=self.drone_ip, port=self.drone_port) 111 | print(f"receiver initialized with ip:{self.drone_ip} and port {self.drone_port}") 112 | self.sender_initialized = False # sender is initialized only when a reset message is received 113 | self.print_screen = print_screen 114 | self.obs_loop_time = obs_loop_time 115 | self.voltage = None 116 | self.min_voltage = None 117 | self.warn_voltage = None 118 | self.max_voltage = None 119 | self.key_cmd_in_progress = False 120 | self.udp_armed = False 121 | self.ludp = 0 122 | self.last_key_tick = time.time() 123 | self.last_obs_tick = time.time() 124 | self.last_bat_tick = time.time() 125 | # reminder: 126 | # 'CH5': DEFAULT_CH5 # DISARMED (1000) / ARMED (1800) 127 | # 'CH6': DEFAULT_CH6 # ANGLE (1000) / NAV_POSHOLD (1500) 128 | # 'CH7': DEFAULT_CH7 # FAILSAFE (1800) 129 | # 'CH8': DEFAULT_CH8 # HEADING HOLD (1800) 130 | self.CMDS = {'roll': DEFAULT_ROLL, 131 | 'pitch': DEFAULT_PITCH, 132 | 'throttle': DEFAULT_THROTTLE, # throttle bellow a certain value disarms the FC 133 | 'yaw': DEFAULT_YAW, 134 | 'aux1': DEFAULT_AUX1, 135 | 'aux2': DEFAULT_AUX2 136 | } 137 | self.CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2'] 138 | self.__batt_state = BATT_NO_INFO 139 | 140 | self.current_flight_command = None 141 | self._flight_origin = None 142 | self.prev_yaw_prime_ts = None 143 | self.prev_alt_prime_ts = None 144 | 145 | self.telemetry = None 146 | self.debug_flags = [] 147 | self._i_obs = 0 148 | self.target_flag = False 149 | self.target_id = None 150 | 151 | # PID values: 152 | 153 | self.pid_limits_xy = (-500, +500) 154 | self.pid_limits_z = (-500, +500) 155 | self.pid_limits_w = (-500, +500) 156 | 157 | self.vel_x_kp = 750.0 158 | self.vel_x_ki = 400.0 159 | self.vel_x_kd = 50.0 160 | 161 | self.vel_y_kp = 750.0 162 | self.vel_y_ki = 400.0 163 | self.vel_y_kd = 50.0 164 | 165 | self.vel_z_kp = 50.0 166 | self.vel_z_ki = 20.0 167 | self.vel_z_kd = 5.0 168 | 169 | self.vel_w_kp = 400.0 # 500 170 | self.vel_w_ki = 200.0 # 100 171 | self.vel_w_kd = 0.0 172 | 173 | self.pid_vel_x = None 174 | self.pid_vel_y = None 175 | self.pid_vel_z = None 176 | self.pid_w_z = None 177 | 178 | self.x_vel_gain = 1 179 | self.y_vel_gain = 1 180 | self.z_vel_gain = 1 181 | self.w_gain = 0.5 182 | 183 | # filters: 184 | 185 | # self.filter_vel_z = Simple1DKalman(err_measure=0.3, q=0.9) 186 | # self.filter_w_z = Simple1DKalman(err_measure=0.2, q=0.9) 187 | self.filter_vel_z = Simple1DExponentialAverage(tau=0.2) 188 | self.filter_w_z = Simple1DExponentialAverage(tau=0.2) 189 | 190 | # tracer: 191 | 192 | self.start_time = time.time() 193 | self.trace_logs = trace_logs 194 | if self.trace_logs: 195 | self.w_tracer = SignalTracer(Path(__file__).parent.resolve() / "trace_logs" / "w_trace.log") 196 | self.z_tracer = SignalTracer(Path(__file__).parent.resolve() / "trace_logs" / "z_trace.log") 197 | self.x_tracer = SignalTracer(Path(__file__).parent.resolve() / "trace_logs" / "x_trace.log") 198 | self.y_tracer = SignalTracer(Path(__file__).parent.resolve() / "trace_logs" / "y_trace.log") 199 | else: 200 | self.w_tracer = None 201 | self.z_tracer = None 202 | self.x_tracer = None 203 | self.y_tracer = None 204 | 205 | def run_curses(self): 206 | result = 1 207 | try: 208 | result = self._controller() 209 | finally: 210 | if result == 1: 211 | print("An error occurred, probably the serial port is not available") 212 | 213 | def _udp_commands_handler(self, command): 214 | """ 215 | Args: 216 | command: a command of the form (str:command, ...:args) 217 | """ 218 | print(f"handling command {command}") 219 | if command[0] == "RES": # reset drone 220 | self.CMDS = {'roll': DEFAULT_ROLL, 221 | 'pitch': DEFAULT_PITCH, 222 | 'throttle': DEFAULT_THROTTLE, # throttle bellow a certain value disarms the FC 223 | 'yaw': DEFAULT_YAW, 224 | 'aux1': DISARMED, # disarmed 225 | 'aux2': DEFAULT_AUX2 226 | } 227 | self.udp_int.init_sender(ip=command[2][0], port=command[2][1]) 228 | self.sender_initialized = True 229 | self._flight_origin = None 230 | self.udp_int.send(pkl.dumps(("RES", (self.drone_ip, self.drone_port)))) 231 | self.current_flight_command = None 232 | elif self.sender_initialized: # do not take any udp order unless reset has been called 233 | identifier = command[1] 234 | if command[0] == "ACT": # action 235 | # hard-reset all PIDs (FIXME: this is to avoid a bug in the PID framework that seems to retain unsuitable values otherwise, correct this in the future) 236 | self.pid_vel_x = PID(kp=self.vel_x_kp, ki=self.vel_x_ki, kd=self.vel_x_kd, sample_time=0.01, output_limits=self.pid_limits_xy, auto_mode=False) 237 | self.pid_vel_y = PID(kp=self.vel_y_kp, ki=self.vel_y_ki, kd=self.vel_y_kd, sample_time=0.01, output_limits=self.pid_limits_xy, auto_mode=False) 238 | self.pid_vel_z = PID(kp=self.vel_z_kp, ki=self.vel_z_ki, kd=self.vel_z_kd, sample_time=0.01, output_limits=self.pid_limits_z, auto_mode=False) 239 | self.pid_w_z = PID(kp=self.vel_w_kp, ki=self.vel_w_ki, kd=self.vel_w_kd, sample_time=0.01, output_limits=self.pid_limits_w, auto_mode=False) 240 | if command[2][0] == "DISARM": 241 | self.CMDS["aux1"] = DISARMED 242 | self.current_flight_command = None 243 | elif command[2][0] == "ARM": 244 | self.CMDS["aux1"] = ARMED 245 | self.CMDS["aux2"] = NAV_POSHOLD_MODE 246 | self.current_flight_command = None 247 | elif command[2][0] == "TAKEOFF": 248 | alt = command[2][1] if command[2][1] is not None else TAKEOFF 249 | self.CMDS["throttle"] = alt # TODO: replace by true altitude (now a throttle value) 250 | self.CMDS["aux2"] = NAV_POSHOLD_MODE 251 | self.current_flight_command = None 252 | elif command[2][0] == "LAND": 253 | self.CMDS["throttle"] = LAND 254 | self.CMDS["aux2"] = NAV_POSHOLD_MODE 255 | self.current_flight_command = None 256 | elif command[2][0] in ("VDF", "VWF"): # velocity 257 | self.current_flight_command = [command[2][0], command[2][1], command[2][2], command[2][3], command[2][4], time.time() + command[2][5]] 258 | elif command[2][0] in ("PDF", "PWF"): # position 259 | self.target_flag = True 260 | self.target_id = identifier 261 | self.current_flight_command = [command[2][0], command[2][1], command[2][2], command[2][3], command[2][4], command[2][5], command[2][6], time.time() + command[2][7]] 262 | elif command[0] == "ST1": # stream on 263 | pass 264 | elif command[0] == "ST0": # stream off 265 | pass 266 | self.udp_int.send(pkl.dumps(("ACK", identifier))) 267 | 268 | def _compute_z_vel(self, pos_z_wf): 269 | if self.prev_alt_prime_ts is None: 270 | self.prev_alt_prime_ts = (pos_z_wf, 0.0, time.time()) 271 | res = 0.0 272 | else: 273 | now = time.time() 274 | dt = now - self.prev_alt_prime_ts[2] 275 | if dt <= 0: 276 | res = self.prev_alt_prime_ts[1] 277 | else: 278 | res = (pos_z_wf - self.prev_alt_prime_ts[0]) / dt 279 | self.prev_alt_prime_ts = (pos_z_wf, res, now) 280 | before_filter = res 281 | res = self.filter_vel_z.update_estimate(res) 282 | if self.trace_logs: 283 | self.z_tracer.write_line(f"{self.prev_alt_prime_ts[2]-self.start_time};{self.prev_alt_prime_ts[0]};{before_filter};{res}") 284 | return res 285 | 286 | def _compute_yaw_rate(self, yaw): 287 | if self.prev_yaw_prime_ts is None: 288 | self.prev_yaw_prime_ts = (yaw, 0.0, time.time()) 289 | res = 0.0 290 | else: 291 | now = time.time() 292 | dt = now - self.prev_yaw_prime_ts[2] 293 | if dt <= 0: 294 | res = self.prev_yaw_prime_ts[1] 295 | else: 296 | # to handle jumps in yaw, we take the smallest abolute value 297 | diff = smallest_angle_diff_rad(yaw, self.prev_yaw_prime_ts[0]) 298 | res = diff / dt 299 | self.prev_yaw_prime_ts = (yaw, res, now) 300 | before_filter = res 301 | res = self.filter_w_z.update_estimate(res) 302 | if self.trace_logs: 303 | self.w_tracer.write_line(f"{self.prev_yaw_prime_ts[2]-self.start_time};{self.prev_yaw_prime_ts[0]};{before_filter};{res}") 304 | return res 305 | 306 | def _flight(self): 307 | """ 308 | This method is a high-level flight controller. 309 | Depending on the current flight command and state of the drone, it outputs relevant MSP commands. 310 | This is done thanks to python PIDs and filters. 311 | In the future, most of this should be integrated directly in the inav flight controller. 312 | Indeed, the use of PIDs here is redundant with what happens in inav. 313 | However, the cognifly version of inav will not take position/velocities as input at the moment but only RPYT, hence this method. 314 | The use of PIDs for velocities should be removed fairly easily, if we can find the mapping from velocities to RPYT. 315 | This is the inverse of a monotonic polynomial for x and y, see inav code (potentially no closed form?). 316 | We have not found the equivalent for z and yaw in inav code yet. 317 | """ 318 | # retrieve the current state 319 | yaw = 0.0 320 | yaw_rate = self._compute_yaw_rate(yaw) 321 | pos_x_wf = 0.0 322 | pos_y_wf = 0.0 323 | vel_x_wf = 0.0 324 | vel_y_wf = 0.0 325 | pos_z_wf = 0.0 326 | vel_z_wf = self._compute_z_vel(pos_z_wf) 327 | 328 | if self._flight_origin is None: # new basis at reset 329 | self._flight_origin = (pos_x_wf, pos_y_wf, yaw) 330 | else: 331 | # change of basis 332 | x_int = pos_x_wf - self._flight_origin[0] 333 | y_int = pos_y_wf - self._flight_origin[1] 334 | cos = np.cos(self._flight_origin[2]) 335 | sin = np.sin(self._flight_origin[2]) 336 | pos_x_wf = x_int * cos + y_int * sin 337 | pos_y_wf = y_int * cos - x_int * sin 338 | vel_x_int = vel_x_wf 339 | vel_y_int = vel_y_wf 340 | vel_x_wf = vel_x_int * cos + vel_y_int * sin 341 | vel_y_wf = vel_y_int * cos - vel_x_int * sin 342 | yaw = smallest_angle_diff_rad(yaw, self._flight_origin[2]) 343 | 344 | cos = np.cos(yaw) 345 | sin = np.sin(yaw) 346 | vel_z_df = vel_z_wf 347 | vel_x_df = vel_x_wf * cos + vel_y_wf * sin 348 | vel_y_df = vel_y_wf * cos - vel_x_wf * sin 349 | 350 | if self.trace_logs: 351 | now = time.time() - self.start_time 352 | self.x_tracer.write_line(f"{now};{pos_x_wf};{vel_x_wf};{vel_x_df}") 353 | self.y_tracer.write_line(f"{now};{pos_y_wf};{vel_y_wf};{vel_y_df}") 354 | 355 | self.telemetry = (self.voltage, pos_x_wf, pos_y_wf, pos_z_wf, yaw, vel_x_wf, vel_y_wf, vel_z_wf, yaw_rate, self.debug_flags) 356 | 357 | if self.current_flight_command is None: 358 | # ensure that rpy commands are at default: 359 | self.CMDS['pitch'] = DEFAULT_PITCH 360 | self.CMDS['roll'] = DEFAULT_ROLL 361 | self.CMDS['yaw'] = DEFAULT_YAW 362 | else: 363 | # handle the current command 364 | if self.current_flight_command[0] in ("VDF", "VWF"): # velocity drone/world frame command 365 | # command is ["VXF", x, y, z, time_end] 366 | time_end = self.current_flight_command[5] 367 | if time.time() >= time_end: 368 | # stop moving and remove flight command 369 | self.pid_vel_x.set_auto_mode(False) 370 | self.pid_vel_y.set_auto_mode(False) 371 | self.pid_vel_z.set_auto_mode(False) 372 | self.pid_w_z.set_auto_mode(False) 373 | self.CMDS['pitch'] = DEFAULT_PITCH 374 | self.CMDS['roll'] = DEFAULT_ROLL 375 | self.CMDS['yaw'] = DEFAULT_YAW 376 | self.current_flight_command = None 377 | else: 378 | if self.current_flight_command[0] == "VDF": # drone frame 379 | v_x = self.current_flight_command[1] 380 | v_y = self.current_flight_command[2] 381 | else: # world frame 382 | v_x_wf = self.current_flight_command[1] 383 | v_y_wf = self.current_flight_command[2] 384 | v_x = v_x_wf * cos + v_y_wf * sin 385 | v_y = v_y_wf * cos - v_x_wf * sin 386 | v_z = self.current_flight_command[3] 387 | w = self.current_flight_command[4] 388 | # adapt pitch, roll and throttle 389 | self.pid_vel_x.setpoint = v_x 390 | self.pid_vel_y.setpoint = v_y 391 | self.pid_vel_z.setpoint = v_z 392 | self.pid_w_z.setpoint = w 393 | self.pid_vel_x.set_auto_mode(True) 394 | self.pid_vel_y.set_auto_mode(True) 395 | if v_z != 0: 396 | self.pid_vel_z.set_auto_mode(True) 397 | else: 398 | self.pid_vel_z.set_auto_mode(False) 399 | if w != 0: 400 | self.pid_w_z.set_auto_mode(True) 401 | else: 402 | self.pid_w_z.set_auto_mode(False) 403 | x_target = self.pid_vel_x(vel_x_df) 404 | y_target = self.pid_vel_y(vel_y_df) 405 | z_target = self.pid_vel_z(vel_z_df) if v_z != 0 else 0 406 | w_target = self.pid_w_z(yaw_rate) if w != 0 else 0 407 | self.CMDS['pitch'] = DEFAULT_PITCH + x_target 408 | self.CMDS['roll'] = DEFAULT_ROLL + y_target 409 | self.CMDS['throttle'] = self.CMDS['throttle'] + z_target 410 | self.CMDS['yaw'] = DEFAULT_YAW + w_target 411 | elif self.current_flight_command[0] == "PWF": # position command 412 | # command is ["PWF", x, y, z, yaw, vel_norm_goal, w_norm_goal, duration] 413 | time_end = self.current_flight_command[7] 414 | if time.time() >= time_end: 415 | # stop moving and remove flight command 416 | self.pid_vel_x.set_auto_mode(False) 417 | self.pid_vel_y.set_auto_mode(False) 418 | self.pid_vel_z.set_auto_mode(False) 419 | self.pid_w_z.set_auto_mode(False) 420 | self.CMDS['pitch'] = DEFAULT_PITCH 421 | self.CMDS['roll'] = DEFAULT_ROLL 422 | self.CMDS['yaw'] = DEFAULT_YAW 423 | self.current_flight_command = None 424 | else: 425 | # compute a 4D vector toward the goal: 426 | x_goal = self.current_flight_command[1] 427 | y_goal = self.current_flight_command[2] 428 | z_goal = self.current_flight_command[3] 429 | yaw_goal = self.current_flight_command[4] 430 | vel_norm_goal = self.current_flight_command[5] 431 | w_norm_goal = self.current_flight_command[6] 432 | x_vector = x_goal - pos_x_wf 433 | y_vector = y_goal - pos_y_wf 434 | z_vector = z_goal - pos_z_wf 435 | yaw_vector = smallest_angle_diff_rad(yaw_goal, yaw) if yaw_goal is not None else None 436 | if self.target_flag: # check whether position target is reached 437 | dist_condition = np.linalg.norm([x_vector, y_vector, z_vector]) <= EPSILON_DIST_TO_TARGET 438 | angle_condition = True if yaw_vector is None else abs(yaw_vector) <= EPSILON_ANGLE_TO_TARGET 439 | if dist_condition and angle_condition: 440 | self.target_flag = False # disable the flag 441 | if self.sender_initialized: 442 | self.udp_int.send(pkl.dumps(("DON", self.target_id))) # notify the remote control 443 | # convert x and y to the drone frame: 444 | x_vector_df = x_vector * cos + y_vector * sin 445 | y_vector_df = y_vector * cos - x_vector * sin 446 | # compute the corresponding desired velocity (non-clipped): 447 | v_x = self.x_vel_gain * x_vector_df 448 | v_y = self.y_vel_gain * y_vector_df 449 | v_z = self.z_vel_gain * z_vector 450 | w = self.w_gain * yaw_vector if yaw_goal is not None else 0.0 451 | # clip to desired velocty: 452 | vel_norm = np.linalg.norm([v_x, v_y, v_z]) 453 | w_norm = abs(w) 454 | if vel_norm > vel_norm_goal: 455 | v_x = v_x * vel_norm_goal / vel_norm 456 | v_y = v_y * vel_norm_goal / vel_norm 457 | v_z = v_z * vel_norm_goal / vel_norm 458 | if w_norm > w_norm_goal: 459 | w = w * w_norm_goal / w_norm 460 | # apply the desired velocity: 461 | self.pid_vel_x.setpoint = v_x 462 | self.pid_vel_y.setpoint = v_y 463 | self.pid_vel_z.setpoint = v_z 464 | self.pid_w_z.setpoint = w 465 | self.pid_vel_x.set_auto_mode(True) 466 | self.pid_vel_y.set_auto_mode(True) 467 | if v_z != 0: 468 | self.pid_vel_z.set_auto_mode(True) 469 | else: 470 | self.pid_vel_z.set_auto_mode(False) 471 | if w != 0: 472 | self.pid_w_z.set_auto_mode(True) 473 | else: 474 | self.pid_w_z.set_auto_mode(False) 475 | x_target = self.pid_vel_x(vel_x_df) 476 | y_target = self.pid_vel_y(vel_y_df) 477 | z_target = self.pid_vel_z(vel_z_df) if v_z != 0 else 0 478 | w_target = self.pid_w_z(yaw_rate) if w != 0 else 0 479 | self.CMDS['pitch'] = DEFAULT_PITCH + x_target 480 | self.CMDS['roll'] = DEFAULT_ROLL + y_target 481 | self.CMDS['throttle'] = self.CMDS['throttle'] + z_target 482 | self.CMDS['yaw'] = DEFAULT_YAW + w_target 483 | elif self.current_flight_command[0] == "PDF": # position command drone frame 484 | # command is ["PDF", x, y, z, yaw, vel_norm_goal, w_norm_goal, duration] 485 | # we convert this into a command in world frame (it will be applied in the next iteration) 486 | current_flight_command = list(self.current_flight_command) 487 | current_flight_command[0] = "PWF" 488 | x_goal_df = current_flight_command[1] 489 | y_goal_df = current_flight_command[2] 490 | x_goal_wf = x_goal_df * cos - y_goal_df * sin + pos_x_wf 491 | y_goal_wf = y_goal_df * cos + x_goal_df * sin + pos_y_wf 492 | yaw_goal_df = current_flight_command[4] 493 | yaw_goal_wf = (yaw + yaw_goal_df) % (2 * np.pi) if yaw_goal_df is not None else None 494 | current_flight_command[1] = x_goal_wf 495 | current_flight_command[2] = y_goal_wf 496 | current_flight_command[4] = yaw_goal_wf 497 | self.current_flight_command = tuple(current_flight_command) 498 | 499 | def _controller(self): 500 | try: 501 | average_cycle = deque([0.0] * NO_OF_CYCLES_AVERAGE_GUI_TIME) 502 | 503 | last_loop_time = last_slow_msg_time = last_cycle_time = time.time() 504 | while True: 505 | start_time = time.time() 506 | 507 | # 508 | # UDP recv non-blocking (NO DELAYS) ----------------------- 509 | # For safety, UDP commands are overridden by key presses 510 | # 511 | if self.udp_int: 512 | udp_cmds = self.udp_int.recv_nonblocking() 513 | if len(udp_cmds) > 0: 514 | for cmd in udp_cmds: 515 | self._udp_commands_handler(pkl.loads(cmd)) 516 | self._flight() 517 | if self.obs_loop_time is not None: 518 | tick = time.time() 519 | if tick - self.last_obs_tick >= self.obs_loop_time and self.sender_initialized: 520 | self.last_obs_tick = tick 521 | self._i_obs += 1 522 | self.udp_int.send(pkl.dumps(("OBS", self._i_obs, self.telemetry))) 523 | # 524 | # end of UDP recv non-blocking ----------------------------- 525 | # 526 | 527 | # 528 | # CLIP RC VALUES ------------------------------------------- 529 | # 530 | self.CMDS['roll'] = clip(self.CMDS['roll'], MIN_CMD_ROLL, MAX_CMD_ROLL) 531 | self.CMDS['pitch'] = clip(self.CMDS['pitch'], MIN_CMD_PITCH, MAX_CMD_PITCH) 532 | self.CMDS['yaw'] = clip(self.CMDS['yaw'], MIN_CMD_YAW, MAX_CMD_YAW) 533 | self.CMDS['throttle'] = clip(self.CMDS['throttle'], MIN_CMD_THROTTLE, MAX_CMD_THROTTLE) 534 | # 535 | # END CLIP RC VALUES --------------------------------------- 536 | # 537 | 538 | # This slows the loop down to CTRL_LOOP_TIME: 539 | end_time = time.time() 540 | last_cycle_time = end_time - start_time 541 | if last_cycle_time < CTRL_LOOP_TIME: 542 | time.sleep(CTRL_LOOP_TIME - last_cycle_time) 543 | 544 | average_cycle.append(last_cycle_time) 545 | average_cycle.popleft() 546 | 547 | finally: 548 | print("Bye!") 549 | 550 | 551 | def run_controller(): 552 | cc = CogniflyControllerDummy() 553 | cc.run_curses() 554 | 555 | 556 | if __name__ == "__main__": 557 | run_controller() 558 | -------------------------------------------------------------------------------- /cognifly/cognifly_controller/trace_logs/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /cognifly/cognifly_remote/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/cognifly/cognifly_remote/__init__.py -------------------------------------------------------------------------------- /cognifly/cognifly_remote/cognifly_remote.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | from threading import Lock, Thread 3 | from queue import Empty 4 | import multiprocessing as mp 5 | import socket 6 | import pickle as pkl 7 | import logging 8 | import time 9 | import numpy as np 10 | from pathlib import Path 11 | 12 | from cognifly.utils.udp_interface import UDPInterface 13 | from cognifly.utils.tcp_video_interface import TCPVideoInterface 14 | from cognifly.utils.ip_tools import extract_ip, get_free_port 15 | from cognifly.utils.functions import clip, get_angle_sequence_rad 16 | 17 | 18 | logger = logging.getLogger(__name__) 19 | logger.setLevel(logging.WARNING) 20 | 21 | 22 | # Constants for the school easy API 23 | 24 | EASY_API_DEFAULT_SPEED = 0.2 # (m/s) 25 | EASY_API_DEFAULT_YAW_RATE = 0.5 # (rad/s) 26 | EASY_API_ADDITIONAL_DURATION = 10.0 # (s) 27 | EASY_API_TAKEOFF_ALTITUDE = 0.5 # should be roughly the same as default takeoff altitude (m) 28 | EASY_API_MAX_ALTITUDE = 0.9 29 | EASY_API_MIN_ALTITUDE = 0.35 30 | 31 | 32 | def _gui_process(name, recv_q, send_q): 33 | """ 34 | Process in charge of managing the Graphical User Interface 35 | 36 | This process communicates with the GUI thread of the Cognifly object 37 | 38 | Args: 39 | name: str: name of the window 40 | recv_q: mutliprocessing.Queue: the process receives tuples of the form ("CMD", value) here 41 | send_q: mutliprocessing.Queue: the process sends tuples of the form ("CMD", value) here 42 | """ 43 | import PySimpleGUI as sg 44 | import cv2 45 | 46 | sprites_folder = Path(__file__).parent.absolute() / 'sprites' 47 | sprite_gamepad_on = str(sprites_folder / 'gamepad_on.png') 48 | sprite_gamepad_off = str(sprites_folder / 'gamepad_off.png') 49 | 50 | sg.theme('BluePurple') 51 | layout = [[sg.Text('Battery:'), 52 | sg.Text('OK', size=(15, 1), key='-batt-'), 53 | sg.Image(filename=sprite_gamepad_off, key='-gamepad-')], 54 | [sg.Button('DISARM')], 55 | [sg.Image(filename='', key='-image-')]] 56 | window = sg.Window(name, 57 | layout, 58 | keep_on_top=True, 59 | enable_close_attempted_event=True) 60 | gamepad_elem = window['-gamepad-'] 61 | image_elem = window['-image-'] 62 | batt_elem = window['-batt-'] 63 | 64 | try: 65 | while True: # Event Loop 66 | # when nothing has been happened, we sleep for a while at the end of the loop: 67 | no_window_event = False 68 | no_read_event = False 69 | 70 | # check window events: 71 | event, values = window.read(0) 72 | if event == 'DISARM': # disarm button pressed 73 | send_q.put(('ARM', False)) 74 | elif event == sg.WINDOW_CLOSE_ATTEMPTED_EVENT and sg.popup_yes_no( 75 | 'You will not be able to open another GUI.\nDo you really want to exit?', 76 | keep_on_top=True) == 'Yes': 77 | send_q.put(('CLOSE', None)) 78 | break 79 | else: 80 | no_window_event = True 81 | 82 | # check recv_q: 83 | try: 84 | cmd, val = recv_q.get_nowait() 85 | except Empty: 86 | cmd, val = None, None 87 | no_read_event = True 88 | 89 | if cmd is not None: 90 | if cmd == 'IMG': 91 | if val is None: # stop display 92 | image_elem.update(data=b'') 93 | else: # display frame 94 | frame = val 95 | imgbytes = cv2.imencode('.png', frame)[1].tobytes() 96 | image_elem.update(data=imgbytes) 97 | elif cmd == 'BAT': # batt str value 98 | batt_str = val 99 | batt_elem.update(batt_str) 100 | elif cmd == 'GMP': # connected gamepad value 101 | gamepad = val 102 | im_gmp = sprite_gamepad_on if gamepad else sprite_gamepad_off 103 | gamepad_elem.update(im_gmp) 104 | 105 | if no_window_event and no_read_event: 106 | time.sleep(0.05) 107 | finally: 108 | # wait for closing acknowledgement: 109 | while True: 110 | try: 111 | cmd, _ = recv_q.get(block=True, timeout=0.1) 112 | except Empty: 113 | break 114 | if cmd == "CLOSE_ACK": 115 | break 116 | window.close() 117 | 118 | 119 | # Remote controller class 120 | 121 | class Cognifly: 122 | """ 123 | Remote UDP controller meant to communicate with a drone that executes cognifly_controller.py 124 | Each command is sent via UDP 125 | The command is sent along an identifier that is sent back by the drone as an aknowledgement 126 | If the acknowledgement of the last sent command is not received after wait_ack_duration seconds, the last command is sent again 127 | Any command received by the drone overrides the previous command instantaneously 128 | """ 129 | def __init__(self, 130 | drone_hostname, 131 | local_hostname=None, 132 | send_port=8988, 133 | recv_port=8989, 134 | recv_port_video=8990, 135 | wait_for_first_obs=True, 136 | wait_for_first_obs_sleep_duration=0.1, 137 | wait_ack_duration=0.2, 138 | gui=True): 139 | """ 140 | Args: 141 | drone_hostname: string: name of the drone (can be an ip address). 142 | local_hostname: string: name of the local machine (can be an ip address). If None, this is retrieved automatically. 143 | send_port: int: udp_send_port for the local UDPInterface. 144 | recv_port: int: udp_recv_port for UDPInterface. 145 | wait_for_first_obs: int: If True, waits for a first observation from the drone on isntantiation. 146 | wait_for_first_obs_sleep_duration: float: used only if wait_for_first_obs is True 147 | wait_ack_duration: float: If > 0.0, the framework will wait this number of seconds for an acknowledgement before resending. 148 | If <= 0.0, the resending feature is disabled. 149 | gui: bool: if True, a gui window will pop when the object is created (emergency disarm, battery, streaming). 150 | """ 151 | self.gui = gui 152 | self.drone_hostname = drone_hostname 153 | self.send_port = send_port 154 | self.recv_port = recv_port 155 | self.recv_port_video = recv_port_video 156 | self.wait_for_first_obs = wait_for_first_obs 157 | self.wait_for_first_obs_sleep_duration = wait_for_first_obs_sleep_duration 158 | self.ea_speed = EASY_API_DEFAULT_SPEED 159 | self.ea_yaw_rate = EASY_API_DEFAULT_YAW_RATE 160 | 161 | self.udp_int = UDPInterface() 162 | self.tcp_video_int = TCPVideoInterface() 163 | 164 | self.local_hostname = socket.gethostname() if local_hostname is None else local_hostname 165 | self.local_ip = socket.gethostbyname(self.local_hostname) if local_hostname is not None else extract_ip() 166 | logger.info(f"local hostname: {self.local_hostname} with IP: {self.local_ip}") 167 | self.drone_hostname = drone_hostname 168 | self.drone_ip = socket.gethostbyname(drone_hostname) 169 | logger.info(f"drone hostname: {self.drone_hostname} with IP: {self.drone_ip}") 170 | 171 | self.udp_int.init_sender(self.drone_ip, self.send_port) 172 | free_port = get_free_port(self.recv_port) 173 | assert free_port is not None, "No available port!" 174 | if free_port != self.recv_port: 175 | logger.warning(f"Port {self.recv_port} is unavailable, trying to communicate on port {free_port} instead.") 176 | self.recv_port = free_port 177 | self.udp_int.init_receiver(self.local_ip, self.recv_port) 178 | 179 | self.wait_ack_duration = wait_ack_duration 180 | self.last_im_id = -1 181 | 182 | self.easy_api_cur_z = 0.0 183 | self.time_takeoff = time.time() 184 | 185 | self._lock = Lock() # used to update the observation and also to resend 186 | self.__obs = None 187 | self.__last_sent_id = 0 188 | self.__last_received_id = 0 189 | self.__last_timestamp = time.time() 190 | self.__last_sent = None 191 | self.__wait_ack_reset = True 192 | self.__last_i_obs = 0 193 | self.__wait_done = False 194 | 195 | self._gui_lock = Lock() 196 | self._gui_display = False 197 | self._gui_batt_str = "OK" 198 | self._gui_gamepad = False 199 | self._gui_process = None 200 | 201 | self._listener_thread = Thread(target=self.__listener_thread) 202 | self._listener_thread.setDaemon(True) # thread will be terminated at exit 203 | self._listener_thread.start() 204 | if self.wait_ack_duration > 0: 205 | self._resender_thread = Thread(target=self.__resender_thread) 206 | self._resender_thread.setDaemon(True) # thread will be terminated at exit 207 | self._resender_thread.start() 208 | else: 209 | self._resender_thread = None 210 | 211 | self.reset() 212 | 213 | with self._gui_lock: 214 | if self.gui: 215 | _gui_thread = Thread(target=self.__gui_thread, args=(self.drone_hostname, )) 216 | _gui_thread.setDaemon(True) # thread will be terminated at exit 217 | _gui_thread.start() 218 | 219 | self.streamoff() 220 | 221 | def __gui_thread(self, gui_name): 222 | """ 223 | Thread in charge of managing the Graphical User Interface 224 | 225 | This thread sends to and receives from the GUI process 226 | 227 | Args: 228 | gui_name: str: the name of the GUI window 229 | """ 230 | mp_ctx = mp.get_context('spawn') # to ensure the 'spawn' context is used on Linux 231 | send_q = mp_ctx.Queue() # recv_q of the process 232 | recv_q = mp_ctx.Queue() # send_q of the process 233 | with self._gui_lock: 234 | self._gui_process = mp_ctx.Process(target=_gui_process, args=(gui_name, send_q, recv_q)) 235 | self._gui_process.start() 236 | is_displaying = False 237 | previous_batt_str = "OK" 238 | previous_gamepad = False 239 | while True: 240 | send_event = False 241 | recv_event = False 242 | # commands from the process: 243 | try: 244 | cmd, val = recv_q.get_nowait() 245 | except Empty: 246 | cmd, val = None, None 247 | if cmd is not None: 248 | recv_event = True 249 | if cmd == 'ARM': 250 | if not val: 251 | self.disarm() 252 | else: 253 | self.arm() 254 | elif cmd == "CLOSE": 255 | send_q.put(('CLOSE_ACK', None)) 256 | with self._gui_lock: 257 | self._gui_process.join() # wait for process end before we close the thread 258 | break 259 | # display: 260 | with self._gui_lock: 261 | gui_display = self._gui_display 262 | if gui_display: 263 | send_event = True # we don't want to wait if display is on, since there is a blocking call 264 | is_displaying = True 265 | with self.tcp_video_int.condition_in: 266 | if self.tcp_video_int.condition_in.wait(timeout=0.1): # wait for new frame 267 | frame, _ = self.tcp_video_int.get_last_image() 268 | else: 269 | frame = None 270 | if frame is not None: 271 | send_q.put(('IMG', frame)) 272 | elif is_displaying: 273 | send_q.put(('IMG', None)) 274 | send_event = True 275 | is_displaying = False 276 | # battery and gamepad display: 277 | with self._gui_lock: 278 | batt_str = self._gui_batt_str 279 | gamepad = self._gui_gamepad 280 | if batt_str != previous_batt_str: 281 | send_q.put(('BAT', batt_str)) 282 | send_event = True 283 | previous_batt_str = batt_str 284 | if gamepad != previous_gamepad: 285 | send_q.put(('GMP', gamepad)) 286 | send_event = True 287 | previous_gamepad = gamepad 288 | # sleep if nothing has occurred: 289 | if not send_event and not recv_event: 290 | time.sleep(0.1) 291 | # close the communication channels: 292 | send_q.close() 293 | recv_q.close() 294 | 295 | def __listener_thread(self): 296 | """ 297 | Thread in charge of receiving UDP messages. 298 | """ 299 | while True: 300 | mes = self.udp_int.recv() 301 | for mb in mes: 302 | m = pkl.loads(mb) 303 | if m[0] == "ACK": # aknowledgement type 304 | self._lock.acquire() 305 | if m[1] > self.__last_received_id: 306 | self.__last_received_id = m[1] # identifier 307 | self._lock.release() 308 | elif m[0] == "OBS": # observation type 309 | self._lock.acquire() 310 | i_obs = m[1] 311 | telemetry = m[2] 312 | gamepad = m[3] 313 | voltage = m[4] 314 | debug_flags = m[5] 315 | if i_obs > self.__last_i_obs: # drop outdated observations 316 | self.__obs = (telemetry, gamepad, voltage, debug_flags) # observation 317 | self.__last_i_obs = i_obs 318 | self._lock.release() 319 | if gamepad is not None: 320 | with self._gui_lock: 321 | self._gui_batt_str = f"{voltage}V" 322 | self._gui_gamepad = gamepad 323 | elif m[0] == "RES": # acknowledgement for the reset command 324 | self._lock.acquire() 325 | self.__wait_ack_reset = False 326 | self._lock.release() 327 | elif m[0] == "DON": # 'done' signal from position control 328 | self._lock.acquire() 329 | if self.__last_sent_id == m[1]: 330 | self.__wait_done = False 331 | self._lock.release() 332 | elif m[0] == "BBT": # bad battery state 333 | pass 334 | # with self._gui_lock: 335 | # self._gui_batt_str = f"{m[1][0]}V" 336 | 337 | def __resender_thread(self): 338 | """ 339 | Thread in charge of resending the last UDP message when it has not been acknowledged by the drone. 340 | """ 341 | while True: 342 | time.sleep(self.wait_ack_duration) 343 | self._lock.acquire() 344 | if self.__last_received_id < self.__last_sent_id: 345 | t = time.time() 346 | if t - self.__last_timestamp > self.wait_ack_duration: 347 | self.udp_int.send(self.__last_sent) # resend (with the same identifier as before) 348 | self.__last_timestamp = t 349 | self._lock.release() 350 | 351 | def send(self, msg_type, msg=None): 352 | """ 353 | Send an UDP message to the drone. 354 | 355 | Args: 356 | msg_type: string in ["RES", ACT"]. 357 | msg: tuple of the form (str:command, ...:args). 358 | """ 359 | self._lock.acquire() 360 | self.__last_sent_id += 1 361 | self.__last_timestamp = time.time() 362 | data = (msg_type, self.__last_sent_id, msg) if msg is not None else (msg_type, self.__last_sent_id) 363 | data_s = pkl.dumps(data) 364 | self.__last_sent = data_s 365 | self.udp_int.send(data_s) 366 | self._lock.release() 367 | 368 | def sleep_until_done(self, max_duration, granularity=0.05): 369 | """ 370 | Sleeps until either max_duration is elapsed or a 'done" signal is received from the position control API. 371 | """ 372 | timeout_t = time.time() + max_duration 373 | while True: 374 | self._lock.acquire() 375 | c = self.__wait_done 376 | self._lock.release() 377 | t = time.time() 378 | if not c or t >= timeout_t: 379 | if c: 380 | self._lock.acquire() 381 | self.__wait_done = False 382 | self._lock.release() 383 | break 384 | time.sleep(granularity) 385 | 386 | def wait_acknowledgement(self, max_duration=1.0, granularity=0.05): 387 | """ 388 | sleeps until an acknowledgement is received from the drone or max_duration is elapsed 389 | """ 390 | timeout_t = time.time() + max_duration 391 | while True: 392 | self._lock.acquire() 393 | c = self.__last_received_id < self.__last_sent_id 394 | self._lock.release() 395 | t = time.time() 396 | if not c or t >= timeout_t: 397 | break 398 | time.sleep(granularity) 399 | 400 | # ================================================================================================================== 401 | 402 | # Pro API 403 | # Use this except for school purpose: 404 | 405 | def reset(self, sleep_duration=0.1): 406 | """ 407 | Resets the drone and waits for reset acknowlegement. 408 | 409 | Args: 410 | sleep_duration: float: the reset command waits for at least this amount of time 411 | """ 412 | self.send(msg_type="RES", msg=(self.local_ip, self.recv_port)) 413 | # wait for the RES message to return 414 | c = True 415 | logger.info("Waiting for reset acknowlegement...") 416 | while c: 417 | time.sleep(sleep_duration) 418 | self._lock.acquire() 419 | c = self.__wait_ack_reset 420 | if not self.__wait_ack_reset: 421 | self.__wait_ack_reset = True # for next reset 422 | self._lock.release() 423 | logger.info("Reset acknowlegement received.") 424 | self._lock.acquire() 425 | self.__obs = None 426 | self.__last_sent_id = 0 427 | self.__last_received_id = 0 428 | self.__last_timestamp = time.time() 429 | self.__last_sent = None 430 | self._lock.release() 431 | if self.wait_for_first_obs: 432 | logger.info("waiting for a first observation from the drone") 433 | while True: 434 | time.sleep(self.wait_for_first_obs_sleep_duration) 435 | self._lock.acquire() 436 | obs = deepcopy(self.__obs) 437 | self._lock.release() 438 | if obs is not None: 439 | break 440 | logger.info("observation initialized") 441 | 442 | def set_coordinates(self, x=None, y=None, yaw=None): 443 | """ 444 | Sets the drone current coordinates. 445 | 446 | This computes a new flight origin. 447 | 448 | Args: 449 | x: float (optional): new coordinate on the X axis (m) 450 | y: float (optional): new coordinate on the Y axis (m) 451 | yaw: float (optional): new coordinate on the yaw axis (rad) 452 | """ 453 | self.send(msg_type="SCO", msg=(x, y, yaw)) 454 | 455 | def set_flight_origin(self, x=None, y=None, yaw=None): 456 | """ 457 | Sets the drone flight origin directly. 458 | 459 | Use with x=0, y=0 and yaw=0 if you wish the origin to be the same as the pose estimator. 460 | This is useful when you are using a custom pose estimator. 461 | Note that calling reset() resets the flight origin to the current position of the drone. 462 | This also happens on instantiation of the Cognifly object. 463 | 464 | Args: 465 | x: float (optional): origin on the X axis (m) 466 | y: float (optional): origin on the Y axis (m) 467 | yaw: float (optional): origin on the yaw axis (rad) 468 | """ 469 | self.send(msg_type="SFO", msg=(x, y, yaw)) 470 | 471 | def arm(self): 472 | """ 473 | Arms the drone. 474 | """ 475 | self.send(msg_type="ACT", msg=("ARM",)) 476 | 477 | def disarm(self): 478 | """ 479 | Disarms the drone. 480 | """ 481 | self.send(msg_type="ACT", msg=("DISARM",)) 482 | 483 | def takeoff_nonblocking(self, altitude=None, track_xy=False, max_duration=10.0, max_velocity=0.5): 484 | """ 485 | The drone takes off. 486 | 487 | Do not use altitude (this currently refers to a FC throttle value and is likely to change in the future). 488 | 489 | Args: 490 | altitude: do not use 491 | track_xy: bool (optional): if True, the drone will track X and Y while taking off 492 | max_duration: float (optional): duration for which X and Y can be tracked if track_xy is True (s) 493 | max_velocity: float (optional): maximum velocity used to track X and Y if track_xy is True (m/s) 494 | """ 495 | self.send(msg_type="ACT", msg=("TAKEOFF", altitude, track_xy, max_duration, max_velocity)) 496 | self.time_takeoff = time.time() 497 | 498 | def land_nonblocking(self): 499 | """ 500 | The drone lands. 501 | """ 502 | self.send(msg_type="ACT", msg=("LAND",)) 503 | 504 | def set_velocity_nonblocking(self, v_x=0.0, v_y=0.0, v_z=0.0, w=0.0, duration=1.0, drone_frame=True): 505 | """ 506 | Sets the drone velocity. 507 | 508 | The drone needs to be armed. 509 | 510 | Args: 511 | v_x: float (optional): velocity target on the frontward axis (m/s); tested range: 0.1 - 0.5 512 | v_y: float (optional): velocity target on the rightward axis (m/s); tested range: 0.1 - 0.5 513 | v_z: float (optional): velocity target on the upward axis (m/s); tested range: 0.05 - 0.1 514 | w: float (optional): yaw rate on the upward axis (rad/s)); tested range: 0.05 - 0.5 515 | duration: float (optional): maximum max_duration of the command (s) 516 | drone_frame: bool (optional): whether to use the drone frame (True) or the world frame (False) 517 | """ 518 | frame_str = "VDF" if drone_frame else "VWF" 519 | self.send(msg_type="ACT", msg=(frame_str, v_x, v_y, v_z, w, duration)) 520 | 521 | def set_position_nonblocking(self, x, y, z=None, yaw=None, max_velocity=0.5, max_yaw_rate=0.5, max_duration=10.0, relative=False, relative_z=False): 522 | """ 523 | Sets a position target for the drone. 524 | If relative is False (default), the target is relative to the world axis. 525 | If relative is True, the target is relative to the drone frame. 526 | Note: z is always in the world frame, except if both relative and relative_z are True. 527 | The drone needs to be armed. 528 | 529 | Args: 530 | x: float: x target (m) 531 | y: float: y target (m) 532 | z: float (optional): z target (m); tested range: 0.3 - 0.9; when None, z is not tracked 533 | yaw: float (optional): yaw target (rad); if None, the yaw is not changed 534 | max_velocity: float (optional): maximum velocity used to go to position (m/s) 535 | max_yaw_rate: float (optional): maximum yaw rate used to go to yaw (rad/s) 536 | max_duration: float (optional): maximum duration of the command (s) 537 | relative: bool (optional): whether to use the drone frame (True) or the world frame (False, default) 538 | relative_z: bool (optional): when both relative and relative_z are True, z is in the drone frame 539 | """ 540 | if relative: 541 | if relative_z: 542 | frame_str = "PDZ" 543 | else: 544 | frame_str = "PDF" 545 | else: 546 | frame_str = "PWF" 547 | self._lock.acquire() 548 | self.__wait_done = True 549 | self._lock.release() 550 | self.send(msg_type="ACT", msg=(frame_str, x, y, z, yaw, max_velocity, max_yaw_rate, max_duration)) 551 | 552 | def set_pid_vel_x(self, k_p=None, k_i=None, k_d=None, timeout=1.0): 553 | """ 554 | Sets the gains of the PID for the x velocity tracker. 555 | 556 | Gains left as None are not affected. 557 | Example of reasonable values: 558 | k_p = 750.0 559 | k_i = 400.0 560 | k_d = 50.0 561 | 562 | Args: 563 | k_p: float (optional): proportional gain 564 | k_i: float (optional): integral gain 565 | k_d: float (optional): derivative gain 566 | timeout: float (optional): timeout when waiting for confirmation from the drone 567 | """ 568 | self.send(msg_type="PVX", msg=(k_p, k_i, k_d)) 569 | self.wait_acknowledgement(max_duration=timeout) 570 | 571 | def set_pid_vel_y(self, k_p=None, k_i=None, k_d=None, timeout=1.0): 572 | """ 573 | Sets the gains of the PID for the y velocity tracker. 574 | 575 | Gains left as None are not affected. 576 | Example of reasonable values: 577 | k_p = 750.0 578 | k_i = 400.0 579 | k_d = 50.0 580 | 581 | Args: 582 | k_p: float (optional): proportional gain 583 | k_i: float (optional): integral gain 584 | k_d: float (optional): derivative gain 585 | timeout: float (optional): timeout when waiting for confirmation from the drone 586 | """ 587 | self.send(msg_type="PVY", msg=(k_p, k_i, k_d)) 588 | self.wait_acknowledgement(max_duration=timeout) 589 | 590 | def set_pid_vel_z(self, k_p=None, k_i=None, k_d=None, timeout=1.0): 591 | """ 592 | Sets the gains of the PID for the z velocity tracker. 593 | 594 | Gains left as None are not affected. 595 | Example of reasonable values: 596 | k_p = 50.0 597 | k_i = 20.0 598 | k_d = 5.0 599 | 600 | Args: 601 | k_p: float (optional): proportional gain 602 | k_i: float (optional): integral gain 603 | k_d: float (optional): derivative gain 604 | timeout: float (optional): timeout when waiting for confirmation from the drone 605 | """ 606 | self.send(msg_type="PVZ", msg=(k_p, k_i, k_d)) 607 | self.wait_acknowledgement(max_duration=timeout) 608 | 609 | def set_pid_vel_w(self, k_p=None, k_i=None, k_d=None, timeout=1.0): 610 | """ 611 | Sets the gains of the PID for the yaw rate tracker. 612 | 613 | Gains left as None are not affected. 614 | Example of reasonable values: 615 | k_p = 400.0 616 | k_i = 200.0 617 | k_d = 0.0 618 | 619 | Args: 620 | k_p: float (optional): proportional gain 621 | k_i: float (optional): integral gain 622 | k_d: float (optional): derivative gain 623 | timeout: float (optional): timeout when waiting for confirmation from the drone 624 | """ 625 | self.send(msg_type="PVW", msg=(k_p, k_i, k_d)) 626 | self.wait_acknowledgement(max_duration=timeout) 627 | 628 | # ================================================================================================================== 629 | 630 | # High-level API for school purpose 631 | # (don't use along the pro API, otherwise weird things will happen with altitude) 632 | # (sleeps after calls, uses cm instead of m, and uses degrees instead of rad): 633 | 634 | def takeoff(self, altitude=None, alt_duration=10.0, track_xy=False, wait_before_control=10.0): 635 | """ 636 | Arms the drone and takes off 637 | 638 | Args: 639 | altitude: float (optional): target altitude (cm) 640 | alt_duration: float (optional): additional max duration to reach the target altitude 641 | track_xy: bool (optional): if True, the drone will track X and Y while taking off 642 | wait_before_control: float (optional): time after takeoff before control starts 643 | """ 644 | altitude = altitude / 100.0 if altitude is not None else EASY_API_TAKEOFF_ALTITUDE 645 | self.reset() 646 | self.arm() 647 | time.sleep(1.0) 648 | # the takeoff altitude might depend on the battery with this and Z will only be defined properly later: 649 | self.takeoff_nonblocking(track_xy=track_xy) 650 | time.sleep(wait_before_control) 651 | self.easy_api_cur_z = altitude 652 | # this works (Z properly defined) but when done from the ground it is a bit violent and not very stable: 653 | self.set_position_nonblocking(x=0.0, y=0.0, z=self.easy_api_cur_z, yaw=0.0, 654 | max_velocity=self.ea_speed, 655 | max_yaw_rate=self.ea_yaw_rate, 656 | max_duration=alt_duration, 657 | relative=False) 658 | self.sleep_until_done(alt_duration) 659 | 660 | def land(self, sleep_duration=5.0): 661 | """ 662 | Lands and disarms the drone 663 | """ 664 | self.easy_api_cur_z = 0.0 665 | self.land_nonblocking() 666 | time.sleep(sleep_duration) 667 | self.disarm() 668 | 669 | def forward(self, val_cm): 670 | """ 671 | Goes forward by number of centimeters 672 | """ 673 | val_m = val_cm / 100.0 674 | duration = val_m / self.ea_speed + EASY_API_ADDITIONAL_DURATION 675 | self.set_position_nonblocking(x=val_m, y=0.0, z=self.easy_api_cur_z, yaw=None, 676 | max_velocity=self.ea_speed, 677 | max_yaw_rate=self.ea_yaw_rate, 678 | max_duration=duration, 679 | relative=True) 680 | self.sleep_until_done(duration) 681 | 682 | def backward(self, val_cm): 683 | """ 684 | Goes backward by number of centimeters 685 | """ 686 | val_m = val_cm / 100.0 687 | duration = val_m / self.ea_speed + EASY_API_ADDITIONAL_DURATION 688 | self.set_position_nonblocking(x=-val_m, y=0.0, z=self.easy_api_cur_z, yaw=None, 689 | max_velocity=self.ea_speed, 690 | max_yaw_rate=self.ea_yaw_rate, 691 | max_duration=duration, 692 | relative=True) 693 | self.sleep_until_done(duration) 694 | 695 | def right(self, val_cm): 696 | """ 697 | Goes right by number of centimeters 698 | """ 699 | val_m = val_cm / 100.0 700 | duration = val_m / self.ea_speed + EASY_API_ADDITIONAL_DURATION 701 | self.set_position_nonblocking(x=0.0, y=val_m, z=self.easy_api_cur_z, yaw=None, 702 | max_velocity=self.ea_speed, 703 | max_yaw_rate=self.ea_yaw_rate, 704 | max_duration=duration, 705 | relative=True) 706 | self.sleep_until_done(duration) 707 | 708 | def left(self, val_cm): 709 | """ 710 | Goes left by number of centimeters 711 | """ 712 | val_m = val_cm / 100.0 713 | duration = val_m / self.ea_speed + EASY_API_ADDITIONAL_DURATION 714 | self.set_position_nonblocking(x=0.0, y=-val_m, z=self.easy_api_cur_z, yaw=None, 715 | max_velocity=self.ea_speed, 716 | max_yaw_rate=self.ea_yaw_rate, 717 | max_duration=duration, 718 | relative=True) 719 | self.sleep_until_done(duration) 720 | 721 | def up(self, val_cm): 722 | """ 723 | Goes up by number of centimeters 724 | """ 725 | val_m = val_cm / 100.0 726 | duration = val_m / self.ea_speed + EASY_API_ADDITIONAL_DURATION 727 | self.easy_api_cur_z += val_m 728 | self.easy_api_cur_z = clip(self.easy_api_cur_z, EASY_API_MIN_ALTITUDE, EASY_API_MAX_ALTITUDE) 729 | self.set_position_nonblocking(x=0.0, y=0.0, z=self.easy_api_cur_z, yaw=None, 730 | max_velocity=self.ea_speed, 731 | max_yaw_rate=self.ea_yaw_rate, 732 | max_duration=duration, 733 | relative=True) 734 | self.sleep_until_done(duration) 735 | 736 | def down(self, val_cm): 737 | """ 738 | Goes down by number of centimeters 739 | """ 740 | val_m = val_cm / 100.0 741 | duration = val_m / self.ea_speed + EASY_API_ADDITIONAL_DURATION 742 | self.easy_api_cur_z -= val_m 743 | self.easy_api_cur_z = clip(self.easy_api_cur_z, EASY_API_MIN_ALTITUDE, EASY_API_MAX_ALTITUDE) 744 | self.set_position_nonblocking(x=0.0, y=0.0, z=self.easy_api_cur_z, yaw=None, 745 | max_velocity=self.ea_speed, 746 | max_yaw_rate=self.ea_yaw_rate, 747 | max_duration=duration, 748 | relative=True) 749 | self.sleep_until_done(duration) 750 | 751 | def cw(self, val_deg): 752 | """ 753 | Rotates clockwise by number of degrees (range 0 - 180). 754 | Note: the drone takes the shortest path, direction is not guaranteed for angles close to 180. 755 | """ 756 | # val_rad = max(val_deg * np.pi / 180.0, 0.0) 757 | # duration = val_rad / self.ea_yaw_rate + EASY_API_ADDITIONAL_DURATION 758 | # seq_angles = get_angle_sequence_rad(val_rad) 759 | # seq = [] 760 | # for sa in seq_angles: 761 | # seq.append([0, 0, self.easy_api_cur_z, sa]) 762 | # self.position_sequence(sequence=seq, max_duration=duration, relative=True) 763 | 764 | val_rad = np.pi if val_deg == 180 else (val_deg % 180) * np.pi / 180.0 765 | duration = val_rad / self.ea_yaw_rate + EASY_API_ADDITIONAL_DURATION 766 | self.set_position_nonblocking(x=0.0, y=0.0, z=self.easy_api_cur_z, yaw=val_rad, 767 | max_velocity=self.ea_speed, 768 | max_yaw_rate=self.ea_yaw_rate, 769 | max_duration=duration, 770 | relative=True) 771 | self.sleep_until_done(duration) 772 | 773 | def ccw(self, val_deg): 774 | """ 775 | Rotates counter-clockwise by number of degrees (range 0 - 180). 776 | Note: the drone takes the shortest path, direction is not guaranteed for angles close to 180. 777 | """ 778 | # val_rad = max(val_deg * np.pi / 180.0, 0.0) 779 | # duration = val_rad / self.ea_yaw_rate + EASY_API_ADDITIONAL_DURATION 780 | # val_rad = - val_rad 781 | # seq_angles = get_angle_sequence_rad(val_rad) 782 | # seq = [] 783 | # for sa in seq_angles: 784 | # seq.append([0, 0, self.easy_api_cur_z, sa]) 785 | # self.position_sequence(sequence=seq, max_duration=duration, relative=True) 786 | 787 | val_rad = np.pi if val_deg == 180 else (val_deg % 180) * np.pi / 180.0 788 | duration = val_rad / self.ea_yaw_rate + EASY_API_ADDITIONAL_DURATION 789 | self.set_position_nonblocking(x=0.0, y=0.0, z=self.easy_api_cur_z, yaw=-val_rad, 790 | max_velocity=self.ea_speed, 791 | max_yaw_rate=self.ea_yaw_rate, 792 | max_duration=duration, 793 | relative=True) 794 | self.sleep_until_done(duration) 795 | 796 | def go(self, x, y, z, yaw=None, max_duration=10.0): 797 | """ 798 | Goes to the requested position (cm) and yaw (deg) targets. 799 | If yaw is None, no yaw target is set. 800 | """ 801 | x = x / 100.0 802 | y = y / 100.0 803 | z = z / 100.0 804 | y = y * np.pi / 180.0 805 | self.easy_api_cur_z = clip(z, EASY_API_MIN_ALTITUDE, EASY_API_MAX_ALTITUDE) 806 | self.set_position_nonblocking(x=x, y=y, z=self.easy_api_cur_z, yaw=yaw, 807 | max_velocity=self.ea_speed, 808 | max_yaw_rate=self.ea_yaw_rate, 809 | max_duration=max_duration, 810 | relative=False) 811 | self.sleep_until_done(max_duration) 812 | 813 | def position_sequence(self, sequence, max_duration=60.0, relative=False, speed=None, yaw_rate=None): 814 | """ 815 | The drone follows a roadmap defined by a sequence of targets. 816 | 817 | Args: 818 | sequence: a sequence of sequence-like elements, each of length 3 or 4 (mixing 3 and 4 is possible). 819 | If the length of an element is 3, it is interpreted as (x, y, z) (cm). 820 | If the length of an element is 4, it is interpreted as (x, y, z, yaw) (cm and deg). 821 | max_duration: float: maximum duration of the whole command. 822 | relative: bool: whether x, y and yaw have to be interpreted in the drone frame (True) 823 | or in the world frame (False, default). 824 | Note: z is always in the world frame. 825 | speed: float: if not None, target speed in cm/s, else the default is used. 826 | yaw_rate: float: if not None, target yaw rate in deg/s, else the default is used. 827 | """ 828 | if speed is not None: 829 | speed = speed / 100.0 # convert to m/s 830 | speed = max(speed, 0.0) 831 | if yaw_rate is not None: 832 | yaw_rate = yaw_rate * np.pi / 180.0 # convert to rad/s 833 | yaw_rate = max(yaw_rate, 0.0) 834 | remaining_duration = max_duration 835 | t_start = time.time() 836 | for elt in sequence: 837 | if 3 <= len(elt) <= 4 and remaining_duration > 0: 838 | x = elt[0] / 100.0 # convert to cm 839 | y = elt[1] / 100.0 # convert to cm 840 | z = elt[2] / 100.0 # convert to cm 841 | yaw = elt[3] * np.pi / 180.0 if len(elt) == 4 else None # convert to rad 842 | 843 | self.easy_api_cur_z = clip(z, EASY_API_MIN_ALTITUDE, EASY_API_MAX_ALTITUDE) 844 | self.set_position_nonblocking(x=x, y=y, z=self.easy_api_cur_z, yaw=yaw, 845 | max_velocity=self.ea_speed if speed is None else speed, 846 | max_yaw_rate=self.ea_yaw_rate if yaw_rate is None else yaw_rate, 847 | max_duration=remaining_duration, 848 | relative=relative) 849 | self.sleep_until_done(remaining_duration) 850 | elapsed_time = time.time() - t_start 851 | remaining_duration -= elapsed_time 852 | 853 | def curve(self, x1, y1, z1, x2, y2, z2, speed): 854 | """ 855 | 2-positions roadmap. 856 | 857 | This is an alias for position_sequence(sequence=[[x1, y1, z1],[x2, y2, z2]], speed=speed). 858 | """ 859 | self.position_sequence(sequence=[[x1, y1, z1],[x2, y2, z2]], speed=speed) 860 | 861 | def set_speed(self, speed): 862 | """ 863 | Sets the maximum speed of the drone in the school API 864 | 865 | Args: 866 | speed: float: speed in cm/s; a reasonable value is 20. 867 | """ 868 | self.ea_speed = speed / 100.0 869 | 870 | def set_yaw_rate(self, yaw_rate): 871 | """ 872 | Sets the maximum yaw rate of the drone in the school API 873 | 874 | Args: 875 | yaw_rate: float: yaw rate in deg/s; a reasonable value is 30. 876 | """ 877 | self.ea_yaw_rate = yaw_rate * np.pi / 180.0 878 | 879 | # ================================================================================================================== 880 | 881 | # Methods common to both APIs: 882 | 883 | def streamon(self, resolution="VGA", fps=10, display=False, wait_first_frame=True, format='jpg', quality=95): 884 | """ 885 | Starts camera streaming. 886 | 887 | CAUTION: This will slow the frequency of the onboard controller down and may make the drone unstable! 888 | The highest resolution and fps are, the worst the influence on the onboard controller. 889 | The image transfers happens over TCP, which comes with a noticeable delay. 890 | CAUTION: High fps may saturate the network in the presence of several drones. 891 | 892 | Args: 893 | resolution: Tuple(float: width, float: height): resolution of the captured images. 894 | fps: integer: maximum framerate (may not be attained), 895 | display: boolean: whether to display the stream in the GUI (requires GUI to be enabled), 896 | wait_first_frame: boolean: whether to sleep until the first frame is available. 897 | format: string: one of 'jpg', 'webp' or 'png' 898 | quality: int: image quality between 0 and 100 899 | """ 900 | if not self.tcp_video_int.is_receiver_running(): 901 | free_port = get_free_port(self.recv_port_video) 902 | assert free_port is not None, "No available port!" 903 | if free_port != self.recv_port_video: 904 | logger.warning(f"Port {self.recv_port_video} is unavailable, trying to communicate on port {free_port} instead.") 905 | self.recv_port_video = free_port 906 | self.tcp_video_int.start_receiver(self.recv_port_video) 907 | time.sleep(1.0) # sleep a bit so the server starts before the drone tries to connect 908 | self.send(msg_type="ST1", msg=(self.local_ip, self.recv_port_video, resolution, fps, format, quality)) 909 | if wait_first_frame: 910 | _ = self.get_frame(wait_new_frame=True) 911 | self._gui_lock.acquire() 912 | self._gui_display = display 913 | self._gui_lock.release() 914 | 915 | def stream(self, resolution="VGA", fps=10, format='jpg', quality=95): 916 | """ 917 | Alias for streamon(resolution, fps=fps, display=True, format=format, quality=quality) 918 | """ 919 | self.streamon(resolution, fps, display=True, format=format, quality=quality) 920 | 921 | def get_frame(self, wait_new_frame=True, timeout=10.0, sleep_between_trials=0.05): 922 | """ 923 | Retrieves a single frame from the stream. 924 | 925 | Args: 926 | wait_new_frame: boolean: if True, two consecutive calls to the fonction will not return identical frames. 927 | timeout: after this duration without retrieving a frame, a TimeoutException will be raised. 928 | sleep_between_trials: float: will sleep this amount of time between two consecutive trials or retrieval. 929 | """ 930 | t_start = time.time() 931 | im = None 932 | im_id = self.last_im_id if wait_new_frame else -1 933 | cond = True 934 | while cond: 935 | im, im_id = self.tcp_video_int.get_last_image() 936 | cond1 = im is None and im_id <= self.last_im_id 937 | cond = cond1 and time.time() - t_start < timeout 938 | if cond: 939 | time.sleep(sleep_between_trials) 940 | else: 941 | self.last_im_id = im_id 942 | if cond1: 943 | raise TimeoutError("Could not retrieve frame from stream.") 944 | return im 945 | 946 | def streamoff(self): 947 | """ 948 | Stops camera streaming 949 | """ 950 | self._gui_lock.acquire() 951 | self._gui_display = False 952 | self._gui_lock.release() 953 | self.send(msg_type="ST0") 954 | time.sleep(1.0) 955 | self.tcp_video_int.stop_receiver() 956 | 957 | def get_time(self): 958 | """ 959 | Gets the number of seconds since the last takeoff command was sent 960 | """ 961 | return time.time() - self.time_takeoff 962 | 963 | def get_telemetry(self): 964 | """ 965 | Gets a tuple that describe the state of the drone in the world frame. 966 | 967 | Angles are in radians. 968 | 969 | Returns: 970 | telemetry: Tuple: (x: float, 971 | y: float, 972 | z: float, 973 | yaw: float, 974 | vel_x: float, 975 | vel_y: float, 976 | vel_z: float, 977 | yaw_rate: float) 978 | gamepad_override: Boolean: if True, the telemtry is invalid! 979 | voltage: float 980 | debug_flags: list of strings 981 | """ 982 | self._lock.acquire() 983 | obs = deepcopy(self.__obs) 984 | self._lock.release() 985 | return obs 986 | 987 | def get_battery(self): 988 | """ 989 | Battery voltage. 990 | 991 | Returns: 992 | battery_voltage: float: battery voltage, in V 993 | """ 994 | return self.get_telemetry()[2] 995 | 996 | def get_position(self): 997 | """ 998 | *Caution:* invalid when gamepad is in override mode. 999 | 1000 | Returns: 1001 | x: float: in cm 1002 | y: float: in cm 1003 | z: float: in cm 1004 | """ 1005 | telemetry = self.get_telemetry()[0] 1006 | return telemetry[0] * 100, telemetry[1] * 100, telemetry[2] * 100 1007 | 1008 | def get_yaw(self): 1009 | """ 1010 | *Caution:* invalid when gamepad is in override mode. 1011 | 1012 | Returns: 1013 | x: float: yaw, in deg 1014 | """ 1015 | telemetry = self.get_telemetry()[0] 1016 | return telemetry[3] * 180.0 / np.pi 1017 | 1018 | def get_velocity(self): 1019 | """ 1020 | *Caution:* invalid when gamepad is in override mode. 1021 | 1022 | Returns: 1023 | v_x: float: in cm/s 1024 | v_y: float: in cm/s 1025 | v_z: float: in cm/s 1026 | """ 1027 | telemetry = self.get_telemetry()[0] 1028 | return telemetry[4] * 100, telemetry[5] * 100, telemetry[6] * 100 1029 | 1030 | def get_health(self): 1031 | """ 1032 | Returns: 1033 | health_flags: list os strings: useful flags for debugging 1034 | """ 1035 | return self.get_telemetry()[3] 1036 | 1037 | def get_yaw_rate(self): 1038 | """ 1039 | *Caution:* invalid when gamepad is in override mode. 1040 | 1041 | Returns: 1042 | w: float: yaw rate in deg/s 1043 | """ 1044 | telemetry = self.get_telemetry()[0] 1045 | return telemetry[7] * 180.0 / np.pi 1046 | 1047 | def get_tof(self): 1048 | """ 1049 | *Caution:* invalid when gamepad is in override mode. 1050 | 1051 | Returns: 1052 | h: float: altitude in cm 1053 | """ 1054 | _, _, res = self.get_position() 1055 | return res 1056 | 1057 | def get_height(self): 1058 | """ 1059 | Alias for get_tof() as cognifly doesn't know its height from the starting point. 1060 | 1061 | *Caution:* invalid when gamepad is in override mode. 1062 | """ 1063 | return self.get_tof() 1064 | 1065 | def get_speed(self): 1066 | """ 1067 | *Caution:* invalid when gamepad is in override mode. 1068 | 1069 | Returns: 1070 | speed: float: norm of the velocity, in cm/s 1071 | """ 1072 | v_x, v_y, v_z = self.get_velocity() 1073 | return np.linalg.norm([v_x, v_y, v_z]) 1074 | -------------------------------------------------------------------------------- /cognifly/cognifly_remote/sprites/gamepad_off.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/cognifly/cognifly_remote/sprites/gamepad_off.png -------------------------------------------------------------------------------- /cognifly/cognifly_remote/sprites/gamepad_on.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/cognifly/cognifly_remote/sprites/gamepad_on.png -------------------------------------------------------------------------------- /cognifly/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/cognifly/utils/__init__.py -------------------------------------------------------------------------------- /cognifly/utils/filters.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | 4 | 5 | class Simple1DKalman: 6 | def __init__(self, err_measure, err_estimate=None, initial_estimate=0, q=0.01): 7 | """ 8 | Args: 9 | err_measure: float: error that can be expected in the measurements (important) 10 | err_estimate: float (optional): error in the initial estimate (if None, same as err_measure) 11 | initial_estimate: float (optional): initial estimate 12 | q: float (optional): process variance (between 0 and 1, depending on how fast the process moves) 13 | """ 14 | self._err_measure = err_measure 15 | self._err_estimate = err_estimate if err_estimate is not None else err_measure 16 | self._last_estimate = initial_estimate 17 | self._q = q 18 | 19 | def update_estimate(self, mea): 20 | kalman_gain = self._err_estimate / (self._err_estimate + self._err_measure) 21 | current_estimate = self._last_estimate + kalman_gain * (mea - self._last_estimate) 22 | self._err_estimate = (1.0 - kalman_gain) * self._err_estimate + self._q * abs(self._last_estimate - current_estimate) 23 | self._last_estimate = current_estimate 24 | return current_estimate 25 | 26 | 27 | class Simple1DExponentialAverage: 28 | def __init__(self, tau, initial_estimate=0): 29 | """ 30 | Args: 31 | tau: float: > 0, time constant of the filter 32 | initial_estimate: float (optional): initial estimate 33 | """ 34 | self.tau = tau 35 | self._last_estimate = initial_estimate 36 | self._last_ts = time.monotonic() 37 | 38 | def _get_alpha(self): 39 | now = time.monotonic() 40 | dt = now - self._last_ts 41 | alpha = 1 - np.exp(- dt / self.tau) 42 | self._last_ts = now 43 | return alpha 44 | 45 | def update_estimate(self, mea): 46 | alpha = self._get_alpha() 47 | current_estimate = alpha * mea + (1 - alpha) * self._last_estimate 48 | self._last_estimate = current_estimate 49 | return current_estimate 50 | -------------------------------------------------------------------------------- /cognifly/utils/functions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import io 3 | 4 | 5 | def clip(x, min_x, max_x): 6 | """ 7 | clips x between min_x and max_x 8 | """ 9 | return max(min(x, max_x), min_x) 10 | 11 | 12 | def get_angle_sequence_rad(angle): 13 | """ 14 | cuts an angle bigger than pi into a sequence of angles <= pi/2 15 | """ 16 | aa = abs(angle) 17 | if aa >= np.pi: # equality is important because the direction is undefined 18 | chunks = int(aa // (np.pi / 2.0)) 19 | last = aa % (np.pi / 2.0) 20 | res = [np.pi / 2.0, ] * chunks 21 | if last != 0: 22 | res.append(last) 23 | else: 24 | res = [aa, ] 25 | res = np.array(res) * np.sign(angle) 26 | return res.tolist() 27 | 28 | 29 | def smallest_angle_diff_rad(theta_new, theta_old): 30 | """ 31 | computes theta_new - theta_old (radian) 32 | """ 33 | diff = theta_new - theta_old 34 | mod_p = diff % (2 * np.pi) 35 | mod_n = diff % (-2 * np.pi) 36 | return mod_p if abs(mod_p) < abs(mod_n) else mod_n 37 | 38 | 39 | def is_raspberrypi(): 40 | try: 41 | with io.open('/sys/firmware/devicetree/base/model', 'r') as m: 42 | if 'raspberry pi' in m.read().lower(): 43 | return True 44 | except Exception: 45 | pass 46 | return False 47 | 48 | 49 | def is_coral(): 50 | try: 51 | with io.open('/sys/firmware/devicetree/base/model', 'r') as m: 52 | if 'coral' in m.read().lower(): 53 | return True 54 | except Exception: 55 | pass 56 | return False 57 | -------------------------------------------------------------------------------- /cognifly/utils/ip_tools.py: -------------------------------------------------------------------------------- 1 | import socket 2 | from psutil import process_iter 3 | import time 4 | import warnings 5 | 6 | 7 | def extract_ip(): 8 | st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 9 | try: 10 | st.connect(('10.255.255.255', 1)) 11 | ip = st.getsockname()[0] 12 | except Exception: 13 | ip = '127.0.0.1' 14 | finally: 15 | st.close() 16 | return ip 17 | 18 | 19 | def get_free_port(port): 20 | initial_port = port 21 | is_free = True 22 | while True: 23 | for proc in process_iter(): 24 | try: 25 | for conns in proc.connections(kind='inet'): 26 | if conns.laddr.port == port: 27 | is_free = False 28 | except: 29 | pass 30 | if is_free: 31 | break 32 | else: 33 | is_free = True 34 | port += 1 35 | if port >= 65000: 36 | port = 2000 37 | if port == initial_port: 38 | return None 39 | return port 40 | -------------------------------------------------------------------------------- /cognifly/utils/joysticks.py: -------------------------------------------------------------------------------- 1 | # Adapted from https://gist.github.com/rdb/8864666 2 | 3 | import os 4 | import struct 5 | import array 6 | from fcntl import ioctl 7 | import warnings 8 | from threading import Thread, Lock 9 | from pathlib import Path 10 | 11 | # These constants were borrowed from linux/input.h 12 | axis_names = { 13 | 0x00: 'x', 14 | 0x01: 'y', 15 | 0x02: 'z', 16 | 0x03: 'rx', 17 | 0x04: 'ry', 18 | 0x05: 'rz', 19 | 0x06: 'throttle', 20 | 0x07: 'rudder', 21 | 0x08: 'wheel', 22 | 0x09: 'gas', 23 | 0x0a: 'brake', 24 | 0x10: 'hat0x', 25 | 0x11: 'hat0y', 26 | 0x12: 'hat1x', 27 | 0x13: 'hat1y', 28 | 0x14: 'hat2x', 29 | 0x15: 'hat2y', 30 | 0x16: 'hat3x', 31 | 0x17: 'hat3y', 32 | 0x18: 'pressure', 33 | 0x19: 'distance', 34 | 0x1a: 'tilt_x', 35 | 0x1b: 'tilt_y', 36 | 0x1c: 'tool_width', 37 | 0x20: 'volume', 38 | 0x28: 'misc', 39 | } 40 | 41 | button_names = { 42 | 0x120: 'trigger', 43 | 0x121: 'thumb', 44 | 0x122: 'thumb2', 45 | 0x123: 'top', 46 | 0x124: 'top2', 47 | 0x125: 'pinkie', 48 | 0x126: 'base', 49 | 0x127: 'base2', 50 | 0x128: 'base3', 51 | 0x129: 'base4', 52 | 0x12a: 'base5', 53 | 0x12b: 'base6', 54 | 0x12f: 'dead', 55 | 0x130: 'a', 56 | 0x131: 'b', 57 | 0x132: 'c', 58 | 0x133: 'x', 59 | 0x134: 'y', 60 | 0x135: 'z', 61 | 0x136: 'tl', 62 | 0x137: 'tr', 63 | 0x138: 'tl2', 64 | 0x139: 'tr2', 65 | 0x13a: 'select', 66 | 0x13b: 'start', 67 | 0x13c: 'mode', 68 | 0x13d: 'thumbl', 69 | 0x13e: 'thumbr', 70 | 71 | 0x220: 'dpad_up', 72 | 0x221: 'dpad_down', 73 | 0x222: 'dpad_left', 74 | 0x223: 'dpad_right', 75 | 76 | # XBox 360 controller uses these codes. 77 | 0x2c0: 'dpad_left', 78 | 0x2c1: 'dpad_right', 79 | 0x2c2: 'dpad_up', 80 | 0x2c3: 'dpad_down', 81 | } 82 | 83 | 84 | class PS4Gamepad: 85 | def __init__(self): 86 | # We'll store the states here. 87 | self._axis_states = {} 88 | self._button_states = {} 89 | self._axis_map = [] 90 | self._button_map = [] 91 | self.fn = Path('/dev/input/js0') 92 | self._lock = Lock() 93 | self._t_js = None 94 | self._js_loop_running = False 95 | self._jsdev = self._get_gamepad() 96 | if self._jsdev is not None: 97 | self._connection() 98 | else: 99 | self._disconnection() 100 | 101 | def _connection(self): 102 | try: 103 | with self._lock: 104 | # Get the device name. 105 | # buf = bytearray(63) 106 | buf = array.array('B', [0] * 64) 107 | ioctl(self._jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len) 108 | self.js_name = buf.tobytes().rstrip(b'\x00').decode('utf-8') 109 | # Get number of axes and buttons. 110 | buf = array.array('B', [0]) 111 | ioctl(self._jsdev, 0x80016a11, buf) # JSIOCGAXES 112 | self.num_axes = buf[0] 113 | buf = array.array('B', [0]) 114 | ioctl(self._jsdev, 0x80016a12, buf) # JSIOCGBUTTONS 115 | self.num_buttons = buf[0] 116 | # Get the axis map. 117 | buf = array.array('B', [0] * 0x40) 118 | ioctl(self._jsdev, 0x80406a32, buf) # JSIOCGAXMAP 119 | for axis in buf[:self.num_axes]: 120 | axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis) 121 | self._axis_map.append(axis_name) 122 | self._axis_states[axis_name] = 0.0 123 | # Get the button map. 124 | buf = array.array('H', [0] * 200) 125 | ioctl(self._jsdev, 0x80406a34, buf) # JSIOCGBTNMAP 126 | for btn in buf[:self.num_buttons]: 127 | btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn) 128 | self._button_map.append(btn_name) 129 | self._button_states[btn_name] = 0 130 | self._connected = True 131 | self._t_js = Thread(target=self._js_loop, daemon=True) 132 | self._t_js.start() 133 | return True 134 | 135 | except Exception as e: 136 | warnings.warn(f"caught exception {str(e)}") 137 | self._disconnection() 138 | return False 139 | 140 | def _disconnection(self): 141 | with self._lock: 142 | self._axis_states = {} 143 | self._button_states = {} 144 | self.js_name = None 145 | self.num_axes = None 146 | self.num_buttons = None 147 | self._axis_map = [] 148 | self._button_map = [] 149 | self._connected = False 150 | self._t_js = None 151 | 152 | def _get_gamepad(self): 153 | if os.path.exists(self.fn): 154 | try: 155 | js = open(self.fn, 'rb') 156 | except Exception as e: 157 | js = None 158 | else: 159 | js = None 160 | return js 161 | 162 | def _js_loop(self): 163 | try: 164 | # Main event loop 165 | while True: 166 | evbuf = self._jsdev.read(8) 167 | if evbuf: 168 | with self._lock: 169 | time, value, type, number = struct.unpack('IhBB', evbuf) 170 | 171 | if type & 0x80: # initial 172 | pass 173 | 174 | if type & 0x01: 175 | button = self._button_map[number] 176 | if button: 177 | self._button_states[button] = value 178 | 179 | if type & 0x02: 180 | axis = self._axis_map[number] 181 | if axis: 182 | fvalue = value / 32767.0 183 | self._axis_states[axis] = fvalue 184 | except OSError as e: 185 | pass # just terminate the thread 186 | except Exception as e: 187 | warnings.warn(f"Caught exception {e}") 188 | finally: 189 | self._disconnection() 190 | 191 | def try_connect(self): 192 | with self._lock: 193 | connected = self._connected 194 | if not connected: 195 | self._jsdev = self._get_gamepad() 196 | if self._jsdev is not None: 197 | connected = self._connection() 198 | return connected 199 | 200 | def get(self): 201 | connected = self.try_connect() 202 | if not connected: 203 | return False, None, None 204 | with self._lock: 205 | connected = self._connected 206 | if connected: 207 | axis_states = self._axis_states.copy() 208 | button_states = self._button_states.copy() 209 | else: 210 | axis_states = None 211 | button_states = None 212 | return connected, axis_states, button_states 213 | 214 | 215 | if __name__ == '__main__': 216 | import time 217 | g = PS4Gamepad() 218 | while True: 219 | print(g.get()) 220 | time.sleep(0.1) 221 | -------------------------------------------------------------------------------- /cognifly/utils/pid.py: -------------------------------------------------------------------------------- 1 | # code adpated from https://github.com/m-lundberg/simple-pid 2 | 3 | import time 4 | import warnings 5 | 6 | 7 | def _clamp(value, limits): 8 | lower, upper = limits 9 | if value is None: 10 | return None 11 | elif (upper is not None) and (value > upper): 12 | return upper 13 | elif (lower is not None) and (value < lower): 14 | return lower 15 | return value 16 | 17 | 18 | try: 19 | # Get monotonic time to ensure that time deltas are always positive 20 | _current_time = time.monotonic 21 | except AttributeError: 22 | # time.monotonic() not available (using python < 3.3), fallback to time.time() 23 | _current_time = time.time 24 | warnings.warn('time.monotonic() not available in python < 3.3, using time.time() as fallback') 25 | 26 | 27 | class PID(object): 28 | """A simple PID controller.""" 29 | 30 | def __init__( 31 | self, 32 | kp=1.0, 33 | ki=0.0, 34 | kd=0.0, 35 | setpoint=0, 36 | sample_time=0.01, 37 | output_limits=(None, None), 38 | auto_mode=True, 39 | proportional_on_measurement=False, 40 | error_map=None, 41 | ): 42 | """ 43 | Initialize a new PID controller. 44 | :param kp: The value for the proportional gain kp 45 | :param ki: The value for the integral gain ki 46 | :param kd: The value for the derivative gain kd 47 | :param setpoint: The initial setpoint that the PID will try to achieve 48 | :param sample_time: The time in seconds which the controller should wait before generating 49 | a new output value. The PID works best when it is constantly called (eg. during a 50 | loop), but with a sample time set so that the time difference between each update is 51 | (close to) constant. If set to None, the PID will compute a new output value every time 52 | it is called. 53 | :param output_limits: The initial output limits to use, given as an iterable with 2 54 | elements, for example: (lower, upper). The output will never go below the lower limit 55 | or above the upper limit. Either of the limits can also be set to None to have no limit 56 | in that direction. Setting output limits also avoids integral windup, since the 57 | integral term will never be allowed to grow outside of the limits. 58 | :param auto_mode: Whether the controller should be enabled (auto mode) or not (manual mode) 59 | :param proportional_on_measurement: Whether the proportional term should be calculated on 60 | the input directly rather than on the error (which is the traditional way). Using 61 | proportional-on-measurement avoids overshoot for some types of systems. 62 | :param error_map: Function to transform the error value in another constrained value. 63 | """ 64 | self.kp, self.ki, self.kd = kp, ki, kd 65 | self.setpoint = setpoint 66 | self.sample_time = sample_time 67 | 68 | self._min_output, self._max_output = None, None 69 | self._auto_mode = auto_mode 70 | self.proportional_on_measurement = proportional_on_measurement 71 | self.error_map = error_map 72 | 73 | self._proportional = 0 74 | self._integral = 0 75 | self._derivative = 0 76 | 77 | self._last_time = None 78 | self._last_output = None 79 | self._last_input = None 80 | 81 | self.output_limits = output_limits 82 | self.reset() 83 | 84 | def __call__(self, input_, dt=None): 85 | """ 86 | Update the PID controller. 87 | Call the PID controller with *input_* and calculate and return a control output if 88 | sample_time seconds has passed since the last update. If no new output is calculated, 89 | return the previous output instead (or None if no value has been calculated yet). 90 | :param dt: If set, uses this value for timestep instead of real time. This can be used in 91 | simulations when simulation time is different from real time. 92 | """ 93 | if not self.auto_mode: 94 | return self._last_output 95 | 96 | now = _current_time() 97 | if dt is None: 98 | dt = now - self._last_time if (now - self._last_time) else 1e-16 99 | elif dt <= 0: 100 | raise ValueError('dt has negative value {}, must be positive'.format(dt)) 101 | 102 | if self.sample_time is not None and dt < self.sample_time and self._last_output is not None: 103 | # Only update every sample_time seconds 104 | return self._last_output 105 | 106 | # Compute error terms 107 | error = self.setpoint - input_ 108 | d_input = input_ - (self._last_input if (self._last_input is not None) else input_) 109 | 110 | # Check if must map the error 111 | if self.error_map is not None: 112 | error = self.error_map(error) 113 | 114 | # Compute the proportional term 115 | if not self.proportional_on_measurement: 116 | # Regular proportional-on-error, simply set the proportional term 117 | self._proportional = self.kp * error 118 | else: 119 | # Add the proportional error on measurement to error_sum 120 | self._proportional -= self.kp * d_input 121 | 122 | # Compute integral and derivative terms 123 | self._integral += self.ki * error * dt 124 | self._integral = _clamp(self._integral, self.output_limits) # Avoid integral windup 125 | 126 | self._derivative = -self.kd * d_input / dt 127 | 128 | # Compute final output 129 | output = self._proportional + self._integral + self._derivative 130 | output = _clamp(output, self.output_limits) 131 | 132 | # Keep track of state 133 | self._last_output = output 134 | self._last_input = input_ 135 | self._last_time = now 136 | 137 | return output 138 | 139 | def __repr__(self): 140 | return ( 141 | '{self.__class__.__name__}(' 142 | 'kp={self.kp!r}, ki={self.ki!r}, kd={self.kd!r}, ' 143 | 'setpoint={self.setpoint!r}, sample_time={self.sample_time!r}, ' 144 | 'output_limits={self.output_limits!r}, auto_mode={self.auto_mode!r}, ' 145 | 'proportional_on_measurement={self.proportional_on_measurement!r},' 146 | 'error_map={self.error_map!r}' 147 | ')' 148 | ).format(self=self) 149 | 150 | @property 151 | def components(self): 152 | """ 153 | The P-, I- and D-terms from the last computation as separate components as a tuple. Useful 154 | for visualizing what the controller is doing or when tuning hard-to-tune systems. 155 | """ 156 | return self._proportional, self._integral, self._derivative 157 | 158 | @property 159 | def tunings(self): 160 | """The tunings used by the controller as a tuple: (kp, ki, kd).""" 161 | return self.kp, self.ki, self.kd 162 | 163 | @tunings.setter 164 | def tunings(self, tunings): 165 | """Set the PID tunings.""" 166 | self.kp, self.ki, self.kd = tunings 167 | 168 | @property 169 | def auto_mode(self): 170 | """Whether the controller is currently enabled (in auto mode) or not.""" 171 | return self._auto_mode 172 | 173 | @auto_mode.setter 174 | def auto_mode(self, enabled): 175 | """Enable or disable the PID controller.""" 176 | self.set_auto_mode(enabled) 177 | 178 | def set_auto_mode(self, enabled, last_output=None): 179 | """ 180 | Enable or disable the PID controller, optionally setting the last output value. 181 | This is useful if some system has been manually controlled and if the PID should take over. 182 | In that case, disable the PID by setting auto mode to False and later when the PID should 183 | be turned back on, pass the last output variable (the control variable) and it will be set 184 | as the starting I-term when the PID is set to auto mode. 185 | :param enabled: Whether auto mode should be enabled, True or False 186 | :param last_output: The last output, or the control variable, that the PID should start 187 | from when going from manual mode to auto mode. Has no effect if the PID is already in 188 | auto mode. 189 | """ 190 | if enabled and not self._auto_mode: 191 | # Switching from manual mode to auto, reset 192 | self.reset() 193 | 194 | self._integral = last_output if (last_output is not None) else 0 195 | self._integral = _clamp(self._integral, self.output_limits) 196 | 197 | self._auto_mode = enabled 198 | 199 | @property 200 | def output_limits(self): 201 | """ 202 | The current output limits as a 2-tuple: (lower, upper). 203 | See also the *output_limits* parameter in :meth:`PID.__init__`. 204 | """ 205 | return self._min_output, self._max_output 206 | 207 | @output_limits.setter 208 | def output_limits(self, limits): 209 | """Set the output limits.""" 210 | if limits is None: 211 | self._min_output, self._max_output = None, None 212 | return 213 | 214 | min_output, max_output = limits 215 | 216 | if (None not in limits) and (max_output < min_output): 217 | raise ValueError('lower limit must be less than upper limit') 218 | 219 | self._min_output = min_output 220 | self._max_output = max_output 221 | 222 | self._integral = _clamp(self._integral, self.output_limits) 223 | self._last_output = _clamp(self._last_output, self.output_limits) 224 | 225 | def reset(self): 226 | """ 227 | Reset the PID controller internals. 228 | This sets each term to 0 as well as clearing the integral, the last output and the last 229 | input (derivative calculation). 230 | """ 231 | self._proportional = 0 232 | self._integral = 0 233 | self._derivative = 0 234 | 235 | self._integral = _clamp(self._integral, self.output_limits) 236 | 237 | self._last_time = _current_time() 238 | self._last_output = None 239 | self._last_input = None 240 | -------------------------------------------------------------------------------- /cognifly/utils/tcp_video_interface.py: -------------------------------------------------------------------------------- 1 | # code inspired from https://picamera.readthedocs.io/en/release-1.13/recipes2.html 2 | 3 | import io 4 | import socket 5 | import struct 6 | import time 7 | import logging 8 | from threading import Thread, Lock, Condition 9 | from copy import deepcopy 10 | import numpy as np 11 | import traceback 12 | import cv2 13 | import pickle as pkl 14 | 15 | from cognifly.utils.ip_tools import get_free_port 16 | from cognifly.utils.functions import is_raspberrypi 17 | 18 | 19 | class SplitFrames(object): 20 | def __init__(self): # , connection 21 | # self.connection = connection 22 | self.stream = io.BytesIO() 23 | self.count = 0 24 | self._condition_out = Condition() 25 | self.__frame = None 26 | self.__framelen = None 27 | 28 | def write(self, buf): 29 | if buf.startswith(b'\xff\xd8'): 30 | # Start of new frame; send the old one's length 31 | # then the data 32 | size = self.stream.tell() 33 | if size > 0: 34 | with self._condition_out: 35 | self.__framelen = size 36 | # self.connection.write(struct.pack(' 100: 150 | compress_quality = 100 151 | encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), compress_quality] 152 | 153 | client_socket = socket.socket() 154 | client_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 155 | client_socket.connect((ip, port)) 156 | connection = client_socket.makefile('wb') 157 | 158 | with self.__lock: 159 | self.__record = True 160 | record = True 161 | if is_raspberrypi(): 162 | cap = cv2.VideoCapture(0) 163 | else: # Coral board 164 | cap = cv2.VideoCapture(1) 165 | assert cap.isOpened(), "VideoCapture could not be opened." 166 | cap.set(cv2.CAP_PROP_FPS, fps) 167 | if resolution != 'VGA': 168 | cap.set(cv2.CAP_PROP_FRAME_WIDTH, resolution[0]) 169 | cap.set(cv2.CAP_PROP_FRAME_HEIGHT, resolution[1]) 170 | ret, f = cap.read() 171 | 172 | while record: 173 | ret, frame = cap.read() 174 | ret, frame = cv2.imencode(compress_format, frame, encode_param) 175 | data = pkl.dumps(frame, 0) 176 | framelen = len(data) 177 | 178 | # send length and frame: 179 | connection.write(struct.pack(' inf_image_i 361 | im_i: int: identifier 362 | """ 363 | with self.__lock: 364 | im_i = self.__image_i 365 | data = deepcopy(self.__image) if im_i > min_image_i else None 366 | if data is not None: 367 | frame = pkl.loads(data, fix_imports=True, encoding="bytes") 368 | im = cv2.imdecode(frame, cv2.IMREAD_COLOR) 369 | # pil_image = data.convert('RGB') 370 | # open_cv_image = np.array(pil_image) 371 | # im = open_cv_image[:, :, ::-1].copy() 372 | else: 373 | im = None 374 | return im, im_i 375 | 376 | def __del__(self): 377 | if self.__sockO: 378 | try: 379 | self.__sockO.close() 380 | except BrokenPipeError: 381 | pass 382 | self.stop_receiver() 383 | -------------------------------------------------------------------------------- /cognifly/utils/udp_interface.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import logging 3 | import platform 4 | 5 | 6 | WINDOWS = platform.system() == 'Windows' 7 | 8 | 9 | class UDPInterface(object): 10 | def __init__(self): 11 | self.ip_send = None 12 | self.port_send = None 13 | self.ip_recv = None 14 | self.port_recv = None 15 | self.__buffersize = None 16 | self.__sockO = None 17 | self.__sockI = None 18 | 19 | def init_sender(self, ip, port): 20 | if self.__sockO is not None: 21 | self.__sockO.close() 22 | self.__sockO = None 23 | self.__sockO = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 24 | self.ip_send = ip 25 | self.port_send = port 26 | 27 | def init_receiver(self, ip, port, clean_buffer=True): 28 | self.ip_recv = ip 29 | self.port_recv = port 30 | if self.__sockI is None: 31 | self.__sockI = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 32 | else: 33 | raise Exception("Cannot initialize the receiver twice.") 34 | self.__sockI.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 35 | self.__sockI.bind((ip, port)) 36 | self.__buffersize = self.__sockI.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF) 37 | logging.debug(f"buffer size: {self.__buffersize}") 38 | while clean_buffer: 39 | logging.debug(f"Cleaning receiving buffer...") 40 | try: 41 | if not WINDOWS: 42 | self.__sockI.recv(1, socket.MSG_DONTWAIT) 43 | else: 44 | self.__sockI.setblocking(False) 45 | self.__sockI.recv(1) 46 | except IOError: # recv raises a error when no data is received 47 | clean_buffer = False 48 | finally: 49 | if WINDOWS: 50 | self.__sockI.setblocking(True) 51 | logging.debug(f"Cleaning receiving buffer...Done!") 52 | 53 | def __del__(self): 54 | if self.__sockI: 55 | self.__sockI.close() 56 | if self.__sockO: 57 | self.__sockO.close() 58 | 59 | def send(self, data): 60 | """ 61 | data: binary string 62 | """ 63 | assert self.__sockO is not None, "sender not initialized" 64 | 65 | self.__sockO.sendto(data, (self.ip_send, self.port_send)) 66 | 67 | def recv(self, timeout=None): 68 | """ 69 | This returns a list of bytestrings (all messages in the buffer) 70 | Returns an empty list if no message is present in the buffer 71 | Reads self.__buffersize bytes in the buffer 72 | """ 73 | assert self.__sockI is not None, "receiver not initialized" 74 | 75 | if timeout: 76 | self.__sockI.settimeout(timeout) 77 | try: 78 | data, _ = self.__sockI.recvfrom(self.__buffersize) 79 | self.__sockI.settimeout(None) 80 | except socket.timeout: 81 | return [] 82 | res = [data] 83 | nb = self.recv_nonblocking() 84 | res = res + nb 85 | return res 86 | 87 | def recv_nonblocking(self): 88 | """ 89 | This returns a list of bytestrings (all messages in the buffer) 90 | Returns an empty list if no message is present in the buffer 91 | Reads everything in the buffer 92 | """ 93 | assert self.__sockI is not None, "receiver not initialized" 94 | 95 | res = [] 96 | while True: 97 | try: 98 | if not WINDOWS: 99 | data, _ = self.__sockI.recvfrom(self.__buffersize, socket.MSG_DONTWAIT) 100 | else: 101 | self.__sockI.setblocking(False) 102 | data, _ = self.__sockI.recvfrom(self.__buffersize) 103 | except IOError: 104 | return res 105 | finally: 106 | if WINDOWS: 107 | self.__sockI.setblocking(True) 108 | res += [data] 109 | 110 | 111 | def test_send(interface, msg): 112 | print(f"sending: {msg}") 113 | interface.send(msg) 114 | 115 | 116 | def test_recv(interface, timeout=None): 117 | print(f"receiving (blocking)...") 118 | res = interface.recv(timeout) 119 | print(f"received: {res}") 120 | 121 | 122 | def test_recv_nonblocking(interface): 123 | print(f"receiving (non blocking)...") 124 | res = interface.recv_nonblocking() 125 | print(f"received: {res}") 126 | 127 | 128 | def main(args): 129 | # standard library imports 130 | import time 131 | 132 | ip_send = args.ipsend 133 | ip_recv = args.iprecv 134 | port_send = args.portsend 135 | port_recv = args.portrecv 136 | 137 | conn = UDPInterface() 138 | conn.init_sender(ip_send, port_send) 139 | conn.init_receiver(ip_recv, port_recv) 140 | 141 | test_send(conn, b"hello 0") 142 | test_send(conn, b"hello 1") 143 | test_recv(conn) 144 | test_send(conn, b"hello 2") 145 | test_send(conn, b"hello 3") 146 | test_recv_nonblocking(conn) 147 | test_recv_nonblocking(conn) 148 | test_send(conn, b"hello 4") 149 | test_recv(conn, timeout=1.0) 150 | test_recv(conn, timeout=1.0) 151 | 152 | 153 | if __name__ == "__main__": 154 | # standard library imports 155 | import argparse 156 | parser = argparse.ArgumentParser() 157 | parser.add_argument('--ipsend', type=str, default="127.0.0.1", help='IP address of the drone if any.') 158 | parser.add_argument('--iprecv', type=str, default="127.0.0.1", help='local IP address if any.') 159 | parser.add_argument('--portsend', type=int, default=8989, help='Port to send udp messages to.') 160 | parser.add_argument('--portrecv', type=int, default=8989, help='Port to reveive udp messages from.') 161 | args = parser.parse_args() 162 | main(args) 163 | -------------------------------------------------------------------------------- /old_controller.py: -------------------------------------------------------------------------------- 1 | 2 | """ 3 | 4 | Cognifly onboard controller 5 | 6 | Features: 7 | a local screen (forwarded by ssh) for monitoring and taking over 8 | a UDP interface for communicating with a high-level UDP controller 9 | 10 | Copyright (C) 2020 Ricardo de Azambuja 11 | 12 | This file is part of YAMSPy. 13 | 14 | YAMSPy is free software: you can redistribute it and/or modify 15 | it under the terms of the GNU General Public License as published by 16 | the Free Software Foundation, either version 3 of the License, or 17 | (at your option) any later version. 18 | 19 | YAMSPy is distributed in the hope that it will be useful, 20 | but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | GNU General Public License for more details. 23 | 24 | You should have received a copy of the GNU General Public License 25 | along with YAMSPy. If not, see . 26 | 27 | WARNING: 28 | This UI is not fit for flying your drone, it is only useful for testing stuff in a safe place, ok? 29 | Acknowledgement: 30 | 31 | This work was possible thanks to the financial support from IVADO.ca (postdoctoral scholarship 2019/2020). 32 | 33 | Disclaimer (adapted from Wikipedia): 34 | None of the authors, contributors, supervisors, administrators, employers, friends, family, vandals, or anyone else 35 | connected (or not) with this project, in any way whatsoever, can be made responsible for your use of the information (code) 36 | contained or linked from here. 37 | 38 | """ 39 | 40 | __author__ = "Ricardo de Azambuja, Yann Bouteiller" 41 | __copyright__ = "Copyright 2020, MISTLab.ca" 42 | __credits__ = [""] 43 | __license__ = "GPL" 44 | __version__ = "0.0.2" 45 | __maintainer__ = "Ricardo de Azambuja" 46 | __email__ = "ricardo.azambuja@gmail.com" 47 | __status__ = "Development" 48 | 49 | import time 50 | import curses 51 | from collections import deque 52 | from itertools import cycle 53 | 54 | # from threading import Timer 55 | 56 | from cognifly.cognifly_controller.YAMSPy.yamspy import MSPy 57 | 58 | # from picamera_AIY_object_detection import MyAIYInterface 59 | 60 | from cognifly.utils.udp_interface import UDPInterface 61 | 62 | SWITCH_MODE_CHR = ord('m') 63 | FORWARD_CHR = ord('8') # curses.KEY_UP 64 | BACKWARD_CHR = ord('5') # curses.KEY_DOWN 65 | LEFT_CHR = ord('7') # curses.KEY_LEFT 66 | RIGHT_CHR = ord('9') # curses.KEY_RIGHT 67 | LEFTYAW_CHR = ord('4') 68 | RIGHTYAW_CHR = ord('6') 69 | UPWARD_CHR = curses.KEY_PPAGE 70 | DOWNWARD_CHR = curses.KEY_NPAGE 71 | PAUSE_CHR = ord('p') 72 | ARM_CHR = ord('a') 73 | DISARM_CHR = ord('d') 74 | REBOOT_CHR = ord('r') 75 | QUIT_CHR = ord('q') 76 | TAKEOFF_CHR = ord('t') 77 | LAND_CHR = ord('l') 78 | 79 | KEY_TIMEOUT = 0.2 # seconds before a keyboard command times out 80 | 81 | DEFAULT_ROLL = 1500 82 | DEFAULT_PITCH = 1500 83 | DEFAULT_THROTTLE = 900 # throttle bellow a certain value disarms the FC 84 | DEFAULT_YAW = 1500 85 | 86 | MIN_CMD_ROLL = 1000 87 | MIN_CMD_PITCH = 1000 88 | MIN_CMD_THROTTLE = 900 # throttle bellow a certain value disarms the FC 89 | MIN_CMD_YAW = 1400 90 | MAX_CMD_ROLL = 2000 91 | MAX_CMD_PITCH = 2000 92 | MAX_CMD_THROTTLE = 2000 93 | MAX_CMD_YAW = 1600 94 | 95 | KEY_N_ROLL = 1350 96 | KEY_N_PITCH = 1350 97 | KEY_N_THROTTLE = -10 # added on throttle(-) key press 98 | KEY_N_YAW = 1450 99 | KEY_P_ROLL = 1650 100 | KEY_P_PITCH = 1650 101 | KEY_P_THROTTLE = 10 # added on throttle(+) key press 102 | KEY_P_YAW = 1550 103 | 104 | KEY_TAKEOFF = 1200 105 | KEY_LAND = 900 106 | 107 | # DEFAULT_CH5 = 1000 # DISARMED (1000) / ARMED (1800) 108 | # DEFAULT_CH6 = 1500 # ANGLE (1000) / NAV_POSHOLD (1500) <= 1300: NAV ALTHOLD 109 | # DEFAULT_CH7 = 1000 # FAILSAFE (1800) 110 | # DEFAULT_CH8 = 1000 # HEADING HOLD (1800) 111 | 112 | DEFAULT_AUX1 = 1000 # DISARMED (1000) / ARMED (1800) 113 | DEFAULT_AUX2 = 1800 # Angle (1000) / NAV_ALTHOLD (1500) / NAV_POSHOLD (1800) 114 | 115 | # Max periods for: 116 | CTRL_LOOP_TIME = 1 / 100 117 | SLOW_MSGS_LOOP_TIME = 1 / 5 # these messages take a lot of time slowing down the loop... 118 | 119 | NO_OF_CYCLES_AVERAGE_GUI_TIME = 10 120 | 121 | 122 | def clip(x, min_x, max_x): 123 | return max(min(x, max_x), min_x) 124 | 125 | 126 | class CogniflyController(): 127 | def __init__(self, 128 | udp_distant_ip=None, 129 | udp_local_ip=None, 130 | udp_distant_port=None, 131 | udp_local_port=None, 132 | print_screen=True, 133 | obs_loop_time=None): 134 | """ 135 | Custom controller and udp interface for Cognifly 136 | If one of udp_distant_ip, udp_distant_port or udp_local_port is None, then the UDP interface is disabled 137 | Args: 138 | udp_distant_ip: string (optional): ip of the udp controller 139 | udp_distant_port: int (optional): port used to send commands to the udp controller 140 | udp_local_port: int (optional): port used to receive commands from the udp controller 141 | print_screen: bool (optional): 142 | if True, messages will be printed repeatedly on the local screen using curses 143 | if False, only the key presses will be read repeatedly using curses 144 | obs_loop_time: float (optional): 145 | if None, an observation is sent by UDP as answer each time a UDP command is received 146 | else, an observation is sent bu UDP every obs_loop_time seconds 147 | """ 148 | if udp_distant_ip is None or udp_local_ip is None or udp_distant_port is None or udp_local_port is None: 149 | self.udp_int = None 150 | else: 151 | self.udp_int = UDPInterface() 152 | self.udp_int.init_receiver(ip=udp_local_ip, port=udp_local_port) 153 | self.udp_int.init_sender(ip=udp_distant_ip, port=udp_distant_port) 154 | self.print_screen = print_screen 155 | self.obs_loop_time = obs_loop_time 156 | self.slow_ctrl_loop_warn_flag = False 157 | self.voltage = 0.0 158 | self.altitude = 0.0 159 | self.key_cmd_in_progress = False 160 | self.udp_armed = False 161 | self.ludp = 0 162 | self.last_key_tick = time.time() 163 | self.last_obs_tick = time.time() 164 | 165 | def run_curses(self): 166 | result = 1 167 | try: 168 | # get the curses screen window 169 | screen = curses.initscr() 170 | # turn off input echoing 171 | curses.noecho() 172 | # respond to keys immediately (don't wait for enter) 173 | curses.cbreak() 174 | # non-blocking 175 | screen.timeout(0) 176 | # map arrow keys to special values 177 | screen.keypad(True) 178 | if self.print_screen: 179 | screen.addstr(1, 0, "Press 'q' to quit, 'r' to reboot, 'm' to change mode, 'a' to arm, 'd' to disarm and arrow keys to control", curses.A_BOLD) 180 | result = self._controller(screen) 181 | finally: 182 | # shut down cleanly 183 | curses.nocbreak() 184 | screen.keypad(0) 185 | curses.echo() 186 | curses.endwin() 187 | if result == 1: 188 | print("An error occurred, probably the serial port is not available") 189 | 190 | def _controller(self, screen): 191 | 192 | # CMDS = {'roll': DEFAULT_ROLL, 193 | # 'pitch': DEFAULT_PITCH, 194 | # 'throttle': DEFAULT_THROTTLE, # throttle bellow a certain value disarms the FC 195 | # 'yaw': DEFAULT_YAW, 196 | # 'CH5': DEFAULT_CH5, # DISARMED (1000) / ARMED (1800) 197 | # 'CH6': DEFAULT_CH6, # ANGLE (1000) / NAV_POSHOLD (1500) 198 | # 'CH7': DEFAULT_CH7, # FAILSAFE (1800) 199 | # 'CH8': DEFAULT_CH8} # HEADING HOLD (1800) 200 | # 201 | # CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'CH5', 'CH6', 'CH7', 'CH8'] 202 | 203 | CMDS = {'roll': DEFAULT_ROLL, 204 | 'pitch': DEFAULT_PITCH, 205 | 'throttle': DEFAULT_THROTTLE, 206 | 'yaw': DEFAULT_YAW, 207 | 'aux1': DEFAULT_AUX1, 208 | 'aux2': DEFAULT_AUX2 209 | } 210 | 211 | CMDS_ORDER = ['roll', 'pitch', 'throttle', 'yaw', 'aux1', 'aux2'] 212 | 213 | # print doesn't work with curses, use addstr instead 214 | 215 | try: 216 | if self.print_screen: 217 | screen.addstr(15, 0, "Connecting to the FC...") 218 | 219 | with MSPy(device="/dev/ttyS0", loglevel='WARNING', baudrate=115200) as board: 220 | if board == 1: # an error occurred... 221 | return 1 222 | 223 | if self.print_screen: 224 | screen.addstr(15, 0, "Connecting to the FC... connected!") 225 | screen.clrtoeol() 226 | screen.move(1, 0) 227 | 228 | average_cycle = deque([0] * NO_OF_CYCLES_AVERAGE_GUI_TIME) 229 | 230 | # It's necessary to send some messages or the RX failsafe will be active 231 | # and it will not be possible to arm. 232 | command_list = ['MSP_API_VERSION', 'MSP_FC_VARIANT', 'MSP_FC_VERSION', 'MSP_BUILD_INFO', 233 | 'MSP_BOARD_INFO', 'MSP_UID', 'MSP_ACC_TRIM', 'MSP_NAME', 'MSP_STATUS', 'MSP_STATUS_EX', 234 | 'MSP_BATTERY_CONFIG', 'MSP_BATTERY_STATE', 'MSP_BOXNAMES'] 235 | 236 | if board.INAV: 237 | command_list.append('MSPV2_INAV_ANALOG') 238 | command_list.append('MSP_VOLTAGE_METER_CONFIG') 239 | 240 | for msg in command_list: 241 | if board.send_RAW_msg(MSPy.MSPCodes[msg], data=[]): 242 | dataHandler = board.receive_msg() 243 | board.process_recv_data(dataHandler) 244 | if board.INAV: 245 | cellCount = board.BATTERY_STATE['cellCount'] 246 | else: 247 | cellCount = 0 # MSPV2_INAV_ANALOG is necessary 248 | min_voltage = board.BATTERY_CONFIG['vbatmincellvoltage'] * cellCount 249 | warn_voltage = board.BATTERY_CONFIG['vbatwarningcellvoltage'] * cellCount 250 | max_voltage = board.BATTERY_CONFIG['vbatmaxcellvoltage'] * cellCount 251 | 252 | if self.print_screen: 253 | screen.addstr(15, 0, "apiVersion: {}".format(board.CONFIG['apiVersion'])) 254 | screen.clrtoeol() 255 | screen.addstr(15, 50, "flightControllerIdentifier: {}".format(board.CONFIG['flightControllerIdentifier'])) 256 | screen.addstr(16, 0, "flightControllerVersion: {}".format(board.CONFIG['flightControllerVersion'])) 257 | screen.addstr(16, 50, "boardIdentifier: {}".format(board.CONFIG['boardIdentifier'])) 258 | screen.addstr(17, 0, "boardName: {}".format(board.CONFIG['boardName'])) 259 | screen.addstr(17, 50, "name: {}".format(board.CONFIG['name'])) 260 | 261 | slow_msgs = cycle(['MSP_ANALOG', 'MSP_STATUS_EX', 'MSP_MOTOR', 'MSP_RC']) 262 | 263 | # send the "drone ready" command to the udp controller: 264 | self.voltage = board.ANALOG['voltage'] 265 | if self.udp_int: 266 | self.udp_int.send_msg([self.voltage, 1.0]) 267 | 268 | cursor_msg = "" 269 | last_loop_time = last_slow_msg_time = last_cycleTime = time.time() 270 | while True: 271 | start_time = time.time() 272 | 273 | # 274 | # UDP recv non-blocking (NO DELAYS) ----------------------- 275 | # For safety, UDP commands are overridden by key presses 276 | # 277 | if self.udp_int: 278 | udp_cmd = self.udp_int.recv_msg_nonblocking() 279 | if udp_cmd is not None: 280 | ludp = len(udp_cmd) 281 | if ludp > 1: 282 | # Warn against CTRL too slow compared to UDP 283 | self.slow_ctrl_loop_warn_flag = True 284 | self.ludp = ludp 285 | cmd = udp_cmd[-1] 286 | # [roll, pitch, yaw, throttle, arm/disarm] (last value not used yet) 287 | if cmd[4] == 1.0 and not self.udp_armed: 288 | self.udp_armed = True 289 | CMDS['aux1'] = 1800 290 | cursor_msg = "UDP_ARMED" 291 | elif cmd[4] != 1.0: 292 | self.udp_armed = False 293 | CMDS['aux1'] = 1000 294 | cursor_msg = "UDP_DISARMED" 295 | else: 296 | CMDS['roll'] = clip(cmd[0], MIN_CMD_ROLL, MAX_CMD_ROLL) 297 | CMDS['pitch'] = clip(cmd[1], MIN_CMD_PITCH, MAX_CMD_PITCH) 298 | CMDS['yaw'] = clip(cmd[2], MIN_CMD_YAW, MAX_CMD_YAW) 299 | CMDS['throttle'] = clip(cmd[3], MIN_CMD_THROTTLE, MAX_CMD_THROTTLE) 300 | if self.obs_loop_time is None: 301 | # answer to the controller: 302 | tick = time.time() 303 | board.fast_read_altitude() 304 | self.altitude = board.SENSOR_DATA['altitude'] 305 | self.udp_int.send_msg([self.altitude, self.voltage, tick]) 306 | if self.obs_loop_time is not None: 307 | tick = time.time() 308 | if tick - self.last_obs_tick >= self.obs_loop_time: 309 | self.last_obs_tick = tick 310 | board.fast_read_altitude() 311 | self.altitude = board.SENSOR_DATA['altitude'] 312 | self.udp_int.send_msg([self.altitude, self.voltage, tick]) 313 | # 314 | # end of UDP recv non-blocking ----------------------------- 315 | # 316 | 317 | char = screen.getch() # get keypress 318 | curses.flushinp() # flushes buffer 319 | # 320 | # KEYS (NO DELAYS) ----------------------------------------- 321 | # 322 | if char != -1: 323 | self.key_cmd_in_progress = True 324 | self.last_key_tick = time.time() 325 | 326 | if char == QUIT_CHR: 327 | break 328 | 329 | elif char == DISARM_CHR: 330 | cursor_msg = 'Disarming...' 331 | CMDS['aux1'] = 1000 332 | 333 | elif char == REBOOT_CHR: 334 | if self.print_screen: 335 | screen.addstr(3, 0, 'Rebooting...') 336 | screen.clrtoeol() 337 | board.reboot() 338 | time.sleep(0.5) 339 | break 340 | 341 | elif char == ARM_CHR: 342 | cursor_msg = 'Arming...' 343 | CMDS['aux1'] = 1800 344 | 345 | elif char == SWITCH_MODE_CHR: 346 | if CMDS['aux2'] <= 1300: 347 | cursor_msg = 'NAV ALTHOLD Mode...' 348 | CMDS['aux2'] = 1500 349 | elif 1700 > CMDS['aux2'] > 1300: 350 | cursor_msg = 'NAV POSHOLD Mode...' 351 | CMDS['aux2'] = 1800 352 | elif CMDS['aux2'] >= 1650: 353 | cursor_msg = 'Angle Mode...' 354 | CMDS['aux2'] = 1000 355 | 356 | elif char == RIGHT_CHR: 357 | # CMDS['roll'] = CMDS['roll'] + 10 if CMDS['roll'] + 10 <= 2000 else CMDS['roll'] 358 | CMDS['roll'] = KEY_P_ROLL 359 | cursor_msg = 'roll(+):{}'.format(CMDS['roll']) 360 | 361 | elif char == LEFT_CHR: 362 | # CMDS['roll'] = CMDS['roll'] - 10 if CMDS['roll'] - 10 >= 1000 else CMDS['roll'] 363 | CMDS['roll'] = KEY_N_ROLL 364 | cursor_msg = 'roll(-):{}'.format(CMDS['roll']) 365 | 366 | elif char == RIGHTYAW_CHR: 367 | CMDS['yaw'] = KEY_P_YAW 368 | cursor_msg = 'yaw(+):{}'.format(CMDS['yaw']) 369 | 370 | elif char == LEFTYAW_CHR: 371 | CMDS['yaw'] = KEY_N_YAW 372 | cursor_msg = 'yaw(-):{}'.format(CMDS['yaw']) 373 | 374 | elif char == FORWARD_CHR: 375 | # CMDS['pitch'] = CMDS['pitch'] + 10 if CMDS['pitch'] + 10 <= 2000 else CMDS['pitch'] 376 | CMDS['pitch'] = KEY_P_PITCH 377 | cursor_msg = 'pitch(+):{}'.format(CMDS['pitch']) 378 | 379 | elif char == BACKWARD_CHR: 380 | # CMDS['pitch'] = CMDS['pitch'] - 10 if CMDS['pitch'] - 10 >= 1000 else CMDS['pitch'] 381 | CMDS['pitch'] = KEY_N_PITCH 382 | cursor_msg = 'pitch(-):{}'.format(CMDS['pitch']) 383 | 384 | elif char == UPWARD_CHR: 385 | CMDS['throttle'] = CMDS['throttle'] + KEY_P_THROTTLE if CMDS['throttle'] + KEY_P_THROTTLE <= MAX_CMD_THROTTLE else CMDS['throttle'] 386 | cursor_msg = 'throttle(+):{}'.format(CMDS['throttle']) 387 | 388 | elif char == DOWNWARD_CHR: 389 | CMDS['throttle'] = CMDS['throttle'] + KEY_N_THROTTLE if CMDS['throttle'] + KEY_N_THROTTLE >= MIN_CMD_THROTTLE else CMDS['throttle'] 390 | cursor_msg = 'throttle(-):{}'.format(CMDS['throttle']) 391 | 392 | elif char == TAKEOFF_CHR: 393 | CMDS['throttle'] = KEY_TAKEOFF 394 | cursor_msg = 'takeoff throttle:{}'.format(CMDS['throttle']) 395 | 396 | elif char == LAND_CHR: 397 | CMDS['throttle'] = KEY_LAND 398 | cursor_msg = 'land throttle:{}'.format(CMDS['throttle']) 399 | 400 | elif PAUSE_CHR: 401 | CMDS['roll'] = DEFAULT_ROLL 402 | CMDS['pitch'] = DEFAULT_PITCH 403 | CMDS['yaw'] = DEFAULT_YAW 404 | 405 | elif self.key_cmd_in_progress: # default behavior 406 | if time.time() - self.last_key_tick >= KEY_TIMEOUT: 407 | self.key_cmd_in_progress = False 408 | CMDS['roll'] = DEFAULT_ROLL 409 | CMDS['pitch'] = DEFAULT_PITCH 410 | CMDS['yaw'] = DEFAULT_YAW 411 | # 412 | # End of KEYS ---------------------------------------------- 413 | # 414 | 415 | # 416 | # IMPORTANT MESSAGES (CTRL_LOOP_TIME based) ---------------- 417 | # 418 | if (time.time() - last_loop_time) >= CTRL_LOOP_TIME: 419 | last_loop_time = time.time() 420 | # Send the RC channel values to the FC 421 | if board.send_RAW_RC([CMDS[ki] for ki in CMDS_ORDER]): 422 | dataHandler = board.receive_msg() 423 | board.process_recv_data(dataHandler) 424 | # 425 | # End of IMPORTANT MESSAGES -------------------------------- 426 | # 427 | 428 | # 429 | # SLOW MSG processing (user GUI and voltage) --------------- 430 | # 431 | if (time.time() - last_slow_msg_time) >= SLOW_MSGS_LOOP_TIME: 432 | last_slow_msg_time = time.time() 433 | # show flags: 434 | if self.slow_ctrl_loop_warn_flag: 435 | if self.print_screen: 436 | screen.addstr(13, 0, f"WARNING: SLOW CTRL / FAST UDP: {self.ludp} udp msgs received at once", curses.A_BOLD + curses.A_BLINK) 437 | screen.clrtoeol() 438 | self.slow_ctrl_loop_warn_flag = False 439 | 440 | next_msg = next(slow_msgs) # circular list 441 | if self.print_screen: # print screen messages 442 | # Read info from the FC 443 | if board.send_RAW_msg(MSPy.MSPCodes[next_msg], data=[]): 444 | dataHandler = board.receive_msg() 445 | board.process_recv_data(dataHandler) 446 | 447 | if next_msg == 'MSP_ANALOG': 448 | self.voltage = board.ANALOG['voltage'] 449 | voltage_msg = "" 450 | if min_voltage < self.voltage <= warn_voltage: 451 | voltage_msg = "LOW BATT WARNING" 452 | elif self.voltage <= min_voltage: 453 | voltage_msg = "ULTRA LOW BATT!!!" 454 | elif self.voltage >= max_voltage: 455 | voltage_msg = "VOLTAGE TOO HIGH" 456 | 457 | screen.addstr(8, 0, "Battery Voltage: {:2.2f}V".format(board.ANALOG['voltage'])) 458 | screen.clrtoeol() 459 | screen.addstr(8, 24, voltage_msg, curses.A_BOLD + curses.A_BLINK) 460 | screen.clrtoeol() 461 | 462 | elif next_msg == 'MSP_STATUS_EX': 463 | ARMED = board.bit_check(board.CONFIG['mode'], 0) 464 | screen.addstr(5, 0, "ARMED: {}".format(ARMED), curses.A_BOLD) 465 | screen.clrtoeol() 466 | 467 | screen.addstr(5, 50, "armingDisableFlags: {}".format(board.process_armingDisableFlags(board.CONFIG['armingDisableFlags']))) 468 | screen.clrtoeol() 469 | 470 | screen.addstr(6, 0, "cpuload: {}".format(board.CONFIG['cpuload'])) 471 | screen.clrtoeol() 472 | screen.addstr(6, 50, "cycleTime: {}".format(board.CONFIG['cycleTime'])) 473 | screen.clrtoeol() 474 | 475 | screen.addstr(7, 0, "mode: {}".format(board.CONFIG['mode'])) 476 | screen.clrtoeol() 477 | 478 | screen.addstr(7, 50, "Flight Mode: {}".format(board.process_mode(board.CONFIG['mode']))) 479 | screen.clrtoeol() 480 | 481 | elif next_msg == 'MSP_MOTOR': 482 | screen.addstr(9, 0, "Motor Values: {}".format(board.MOTOR_DATA)) 483 | screen.clrtoeol() 484 | 485 | elif next_msg == 'MSP_RC': 486 | screen.addstr(10, 0, "RC Channels Values: {}".format(board.RC['channels'])) 487 | screen.clrtoeol() 488 | 489 | screen.addstr(11, 0, "GUI cycleTime: {0:2.2f}ms (average {1:2.2f}Hz)".format((last_cycleTime) * 1000, 1 / (sum(average_cycle) / len(average_cycle)))) 490 | screen.clrtoeol() 491 | 492 | screen.addstr(3, 0, cursor_msg) 493 | screen.clrtoeol() 494 | else: # no message printing: 495 | if next_msg == 'MSP_ANALOG': 496 | self.voltage = board.ANALOG['voltage'] 497 | # 498 | # end of SLOW MSG ------------------------------------------ 499 | # 500 | 501 | # This slows the loop down to CTRL_LOOP_TIME: 502 | end_time = time.time() 503 | last_cycleTime = end_time - start_time 504 | if last_cycleTime < CTRL_LOOP_TIME: 505 | time.sleep(CTRL_LOOP_TIME - last_cycleTime) 506 | 507 | average_cycle.append(last_cycleTime) 508 | average_cycle.popleft() 509 | 510 | finally: 511 | if self.print_screen: 512 | screen.addstr(5, 0, "Disconnected from the FC!") 513 | screen.clrtoeol() 514 | print("Bye!") 515 | 516 | 517 | def main(args): 518 | ip_send = args.ipsend 519 | ip_recv = args.iprecv 520 | port_send = args.portsend 521 | port_recv = args.portrecv 522 | obs_loop_time = args.obslooptime 523 | cc = CogniflyController(udp_distant_ip=ip_send, udp_local_ip=ip_recv, udp_distant_port=port_send, udp_local_port=port_recv, obs_loop_time=obs_loop_time) 524 | cc.run_curses() 525 | 526 | 527 | if __name__ == "__main__": 528 | import argparse 529 | parser = argparse.ArgumentParser() 530 | parser.add_argument('--ipsend', type=str, default=None, help='IP address of the drone if any.') 531 | parser.add_argument('--iprecv', type=str, default=None, help='local IP address if any.') 532 | parser.add_argument('--portsend', type=int, default=8989, help='Port to send udp messages to.') 533 | parser.add_argument('--portrecv', type=int, default=8989, help='Port to reveive udp messages from.') 534 | parser.add_argument('--obslooptime', type=float, default=None, help='Duration between sending UDP observations.') 535 | args = parser.parse_args() 536 | main(args) 537 | -------------------------------------------------------------------------------- /readme/DRONE_SETUP.md: -------------------------------------------------------------------------------- 1 | # Drone setup 2 | 3 | Follow these step-by-step instructions to properly setup the CogniFly drone from zero. 4 | 5 | ## Flight controller configuration 6 | - download both the [cognifly framework](https://github.com/thecognifly/cognifly-python/releases/download/v0.0.4/cognifly_framework.hex) and the [cognifly configuration](https://github.com/thecognifly/cognifly-python/releases/download/v0.0.4/cognifly_configuration.txt) files 7 | - connect the flight controller to your PC through USB 8 | - launch [inav-configurator 2.3.2](https://github.com/iNavFlight/inav-configurator/releases/tag/2.3.2) (on linux, do it with `sudo`) 9 | - `connect` to the flight controller 10 | - open the `CLI` tab and enter the following command: 11 | ```terminal 12 | dfu 13 | ``` 14 | This will disconnect the flight controller and restart it in DFU mode. 15 | - open the `firmware flasher` tab 16 | - make sure `full chip erase` is selected 17 | - click `load firmware [local]` 18 | - select the cognifly framework file previously downloaded (`.hex` file) 19 | - click `flash firmware` and wait for completion 20 | - `connect` to the flight controller, select keep defaults, let it reboot, `connect` again 21 | - open the `CLI` tab and select `load from file` 22 | - navigate to the cognifly configuration file previously downloaded (`.txt` file) 23 | - click `execute` and wait for completion 24 | - _(NB: check for errors in the logs and ensure the whole .txt has been processed)_ 25 | - enter the following command: 26 | ```terminal 27 | save 28 | ``` 29 | This will reboot the flight controller. 30 | - `connect` to the flight controller again 31 | - in the `setup` tab, check that everything is green 32 | - (optional) go to the `calibration` tab and follow the calibration instructions 33 | - go to the `mixer` tab and [follow the inav instructions](https://github.com/iNavFlight/inav/blob/master/docs/Mixer.md) to configure the 4 motors properly 34 | - `disconnect` from the flight controller 35 | 36 | ## Raspberry Pi configuration 37 | 38 | ### Raspbian image: 39 | 40 | The easiest way of using `cognifly-python` is to flash our readily set up Rasbian image on your SD card. 41 | 42 | - download the [Cognifly Raspbian image](https://github.com/thecognifly/cognifly-python/releases/download/v0.2.1/cognifly_image.tar.gz) 43 | - extract the archive 44 | - you should now see the extracted file named `cognifly.img` 45 | - flash `cognifly.img` on your SD card (e.g., using [Win32 Disk Imager](https://win32diskimager.download/) on Windows, or `dd` on Linux) 46 | - in the now flashed SD card, configure wpa-supplicant so the drone connects to your router: 47 | - edit the `/rootfs/etc/wpa_supplicant/wpa_supplicant.conf` file (on linux, this may require `sudo`) 48 | - adapt the following to your WiFi network in the `wpa_supplicant.conf` file: 49 | ```terminal 50 | network={ 51 | ssid="yourNetworkSSID" 52 | psk="yourNetworkPassword" 53 | scan_ssid=1 54 | } 55 | ``` 56 | - change the name of the drone: 57 | - edit the `/rootfs/etc/hostname` file 58 | - replace the default `K00` by your drone name 59 | - edit the `/rootfs/etc/hosts` file 60 | - replace the default `K00` in the last line by your drone name 61 | - plug the SD card into the Raspberry Pi 62 | - plug a charged battery to start the drone 63 | - password for the user **pi**: `maplesyrup` 64 | 65 | ### Manual installation: 66 | 67 | _(Note: if using the Raspbian image, you can ignore this section.)_ 68 | 69 | In case you wish to install the library on your own Raspbian: 70 | 71 | - install [raspbian](https://www.raspberrypi.org/software/operating-systems/) on the SD card (you can use Raspberry Pi OS Lite) 72 | 73 | _(Note: You can use [Raspberry Pi Imager](https://www.raspberrypi.com/software/) to install raspbian, which provides a GUI to skip most of the following.)_ 74 | 75 | The SD card should now have two partitions: `boot` and `rootfs` 76 | 77 | - configure [SSH access](https://phoenixnap.com/kb/enable-ssh-raspberry-pi): 78 | - create an empty file named `ssh` at the root of the `/boot/` partition 79 | 80 | - configure wpa-supplicant so the drone connects to your router: 81 | - edit the `/rootfs/etc/wpa_supplicant/wpa_supplicant.conf` file (on linux, this may require `sudo`) 82 | - adapt the following and add it to the end of the `wpa_supplicant.conf` file: 83 | ```terminal 84 | network={ 85 | ssid="yourNetworkSSID" 86 | psk="yourNetworkPassword" 87 | scan_ssid=1 88 | } 89 | ``` 90 | 91 | - change the name of the drone: 92 | - edit the `/rootfs/etc/hostname` file 93 | - replace the default `raspberrypi` by your drone name 94 | - edit the `/rootfs/etc/hosts` file 95 | - replace the default `raspberrypi` in the last line by your drone name 96 | 97 | - enable the pi camera and the UART serial port: 98 | - edit the `/boot/config.txt` file 99 | - under the `[all]` section, set the following lines: 100 | ```terminal 101 | enable_uart=1 # serial port 102 | start_x=1 # camera 103 | gpu_mem=128 # camera 104 | ``` 105 | 106 | - disable the console messages in the serial port: 107 | - edit the `/boot/cmdline.txt` file 108 | - if you see `console=serial0,115200`, remove this command 109 | - plug the SD card into the Raspberry Pi 110 | - plug a charged battery to start the drone 111 | - wait for the Raspberry PI to boot (the light should stop blinking) 112 | - ssh the drone from your computer (the default password should be `raspberry`): 113 | ```terminal 114 | ssh pi@myDroneName.local 115 | ``` 116 | - execute the following on the Raspberry Pi: 117 | ```terminal 118 | sudo apt-get update 119 | sudo apt-get install libatlas-base-dev libopenjp2-7 libtiff5 python3-pip 120 | ``` 121 | 122 | You can now install the `cognifly-python` library on the drone. 123 | -------------------------------------------------------------------------------- /readme/figures/gui.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/readme/figures/gui.PNG -------------------------------------------------------------------------------- /readme/figures/ps4.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thecognifly/cognifly-python/52648e5ad7ee8898d1b78922953dc4d4a07293f4/readme/figures/ps4.PNG -------------------------------------------------------------------------------- /scripts/cognifly-controller: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # launch the onboard python controller for the cognifly remote control library 4 | # usage: 5 | # cognifly-controller [--headless] 6 | # 7 | 8 | set -e # exit in case somethings fails 9 | 10 | echo "Starting cognifly_controller.py" # echo the original command 11 | 12 | HEADLESS=false 13 | 14 | for i in "$@" ; do 15 | if [[ $i == "headless" ]] ; then 16 | HEADLESS=true 17 | fi 18 | done 19 | 20 | if $HEADLESS 21 | then 22 | echo "using headless mode" 23 | python3 -m cognifly controller-headless 24 | else 25 | echo "using screen mode" 26 | python3 -m cognifly controller 27 | fi 28 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | description-file = README.md 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import io 4 | from setuptools import setup, find_packages 5 | 6 | if sys.version_info < (3, 7): 7 | sys.exit('Sorry, Python < 3.7 is not supported.') 8 | 9 | 10 | def is_raspberrypi(): 11 | try: 12 | with io.open('/sys/firmware/devicetree/base/model', 'r') as m: 13 | if 'raspberry pi' in m.read().lower(): 14 | return True 15 | except Exception: 16 | pass 17 | return False 18 | 19 | 20 | def is_coral(): 21 | try: 22 | with io.open('/sys/firmware/devicetree/base/model', 'r') as m: 23 | if 'coral' in m.read().lower(): 24 | return True 25 | except Exception: 26 | pass 27 | return False 28 | 29 | 30 | with open("README.md", "r") as fh: 31 | long_description = fh.read() 32 | 33 | deps = ['numpy', 'Pillow', 'psutil'] 34 | if is_raspberrypi(): # drone 35 | deps.append(['picamera', 'yamspy==0.3.3', 'opencv-python']) 36 | elif is_coral(): # drone 37 | deps.append(['yamspy==0.3.3', 'opencv-python']) 38 | else: # remote 39 | deps.append(['opencv-contrib-python', 'pysimplegui']) 40 | 41 | setup(name='cognifly', 42 | packages=[package for package in find_packages()], 43 | version='0.3.3', 44 | license='MIT', 45 | description='Control the CogniFly open-source drone from python', 46 | long_description=long_description, 47 | long_description_content_type="text/markdown", 48 | author='Yann Bouteiller', 49 | url='https://github.com/thecognifly/cognifly-python', 50 | download_url='https://github.com/thecognifly/cognifly-python/archive/refs/tags/v0.3.3.tar.gz', 51 | keywords=['cognifly', 'drone', 'remote', 'control'], 52 | install_requires=deps, 53 | scripts=["scripts/cognifly-controller", ], 54 | classifiers=[ 55 | 'Development Status :: 4 - Beta', 56 | 'Intended Audience :: Developers', 57 | 'Intended Audience :: Education', 58 | 'Intended Audience :: Information Technology', 59 | 'Intended Audience :: Science/Research', 60 | 'License :: OSI Approved :: MIT License', 61 | 'Operating System :: Microsoft :: Windows', 62 | 'Operating System :: POSIX :: Linux', 63 | 'Programming Language :: Python', 64 | 'Framework :: Robot Framework :: Library', 65 | 'Topic :: Education', 66 | 'Topic :: Scientific/Engineering :: Artificial Intelligence', 67 | ], 68 | package_data={'cognifly': [ 69 | 'cognifly_remote/sprites/gamepad_off.png', 70 | 'cognifly_remote/sprites/gamepad_on.png']} 71 | ) 72 | --------------------------------------------------------------------------------