pax_global_header 0000666 0000000 0000000 00000000064 15043245150 0014511 g ustar 00root root 0000000 0000000 52 comment=932f6945cfc7fbc6579e166667d43e327fe44f2d
MAVProxy-1.8.74/ 0000775 0000000 0000000 00000000000 15043245150 0013277 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/.flake8 0000664 0000000 0000000 00000000656 15043245150 0014461 0 ustar 00root root 0000000 0000000 [flake8]
ignore =
# H301: one import per line
H301,
# H306: imports not in alphabetical order (time, os)
H306,
# E226: missing whitespace around arithmetic operator
E226,
# E261 at least two spaces before inline comment
E261,
# W504 line break after binary operator
W504,
# E203 whitespace before ':'
E203,
# E221 multiple spaces before operator
E221
max-line-length = 127
MAVProxy-1.8.74/.github/ 0000775 0000000 0000000 00000000000 15043245150 0014637 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/.github/dependabot.yml 0000664 0000000 0000000 00000001103 15043245150 0017462 0 ustar 00root root 0000000 0000000 # Keep GitHub Actions up to date with GitHub's Dependabot...
# https://docs.github.com/en/code-security/dependabot/working-with-dependabot/keeping-your-actions-up-to-date-with-dependabot
# https://docs.github.com/en/code-security/dependabot/dependabot-version-updates/configuration-options-for-the-dependabot.yml-file#package-ecosystem
version: 2
updates:
- package-ecosystem: github-actions
directory: /
groups:
github-actions:
patterns:
- "*" # Group all Actions updates into a single larger pull request
schedule:
interval: weekly
MAVProxy-1.8.74/.github/workflows/ 0000775 0000000 0000000 00000000000 15043245150 0016674 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/.github/workflows/python-cleanliness.yml 0000664 0000000 0000000 00000001012 15043245150 0023230 0 ustar 00root root 0000000 0000000 name: test Python cleanliness
on: [push, pull_request, workflow_dispatch]
concurrency:
group: ci-${{github.workflow}}-${{ github.ref }}
cancel-in-progress: true
jobs:
build:
runs-on: ubuntu-22.04
steps:
# git checkout the PR
- uses: actions/checkout@v4
with:
submodules: 'recursive'
- name: Install dependencies
run: |
python -m pip install -U flake8
- name: Check Python with Flake8
run: |
scripts/run_flake8.py MAVProxy
MAVProxy-1.8.74/.github/workflows/python-publish.yml 0000664 0000000 0000000 00000001732 15043245150 0022407 0 ustar 00root root 0000000 0000000 # This workflow will upload a Python Package using Twine when a release is created
# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries
name: Upload MAVProxy Package
on:
release:
types: [published]
jobs:
deploy:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
- name: Set up Python
uses: actions/setup-python@v5
with:
python-version: '3.10'
- name: Install dependencies
run: |
python -m pip install -U pip
python -m pip install -U wheel
python -m pip install -U pymavlink
pip install build
pip install -U .
git submodule update --recursive --init
- name: Build package
run: python -m build
- name: Publish package
uses: pypa/gh-action-pypi-publish@release/v1
with:
user: __token__
password: ${{ secrets.PYPI_API_TOKEN }}
MAVProxy-1.8.74/.github/workflows/windows_build.yml 0000664 0000000 0000000 00000006520 15043245150 0022273 0 ustar 00root root 0000000 0000000 name: Windows Build
on: [push, pull_request, workflow_dispatch]
concurrency:
group: ci-${{github.workflow}}-${{ github.ref }}
cancel-in-progress: true
jobs:
build:
runs-on: 'windows-latest'
strategy:
matrix:
python-version: ['3.10']
steps:
- uses: actions/checkout@v4
with:
submodules: 'recursive'
- uses: actions/checkout@v4
- name: Set up Python ${{ matrix.python-version }}
uses: actions/setup-python@v5
with:
python-version: ${{ matrix.python-version }}
- name: Install dependencies
run: |
python3 -m pip install -U wheel setuptools pip
python3 -m pip install -U pywin32 lxml pymavlink numpy matplotlib pyserial opencv-python PyYAML Pygame Pillow wxpython prompt-toolkit scipy
python3 -m pip install -U openai pyaudio
python3 -m pip install -U pyinstaller==6.7.0 packaging
- name: Download Inno Setup installer
run: curl -L -o installer.exe http://files.jrsoftware.org/is/6/innosetup-6.3.1.exe
- name: Install Inno Setup
run: ./installer.exe /verysilent /allusers /dir=inst
- name: Build MAVProxy
run: |
python3 -m pip install .[recommended] --user
python3 -m pip list
- name: Prepare installer
run: |
cd MAVProxy
copy ..\\windows\\mavproxy.spec
pyinstaller --clean mavproxy.spec
del mavproxy.spec
- name: Download parameters
run: |
mkdir Parameters
$wc = New-Object System.Net.WebClient
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/Rover/apm.pdef.xml', 'Parameters\Rover.xml')
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/Copter/apm.pdef.xml', 'Parameters\Copter.xml')
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/Plane/apm.pdef.xml', 'Parameters\Plane.xml')
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/Sub/apm.pdef.xml', 'Parameters\Sub.xml')
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/AntennaTracker/apm.pdef.xml', 'Parameters\AntennaTracker.xml')
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/Heli/apm.pdef.xml', 'Parameters\Heli.xml')
$wc.DownloadFile('https://autotest.ardupilot.org/Parameters/Blimp/apm.pdef.xml', 'Parameters\Blimp.xml')
- name: Build installer
run: |
cd windows
$env:VERSION=$(python returnVersion.py)
python3 returnVersion.py > version.txt
ISCC.exe /dMyAppVersion=$env:VERSION mavproxy.iss
ls Output
- name: Archive build
uses: actions/upload-artifact@v4
with:
name: MAVProxyInstaller
path: windows/Output
retention-days: 7
- name: Pre Release
uses: "softprops/action-gh-release@v2"
if: github.ref == 'refs/heads/master'
with:
prerelease: true
name: "Development Build"
tag_name: "latest"
files: windows/Output/*.*
token: "${{ secrets.GITHUB_TOKEN }}"
- name: Release
uses: "softprops/action-gh-release@v2"
if: startsWith(github.ref, 'refs/tags/v')
with:
prerelease: false
files: windows/Output/*.*
token: "${{ secrets.GITHUB_TOKEN }}"
MAVProxy-1.8.74/.gitignore 0000664 0000000 0000000 00000000215 15043245150 0015265 0 ustar 00root root 0000000 0000000 mavproxy
*~
*.o
*.pyc
*.log
camera/
build/
dist/
gtest/
*.tlog
*.raw
MANIFEST
*.egg-info
.DS_Store
*.bak
*.orig
windows/version.txt
/.vscode
MAVProxy-1.8.74/.gitmodules 0000664 0000000 0000000 00000000212 15043245150 0015447 0 ustar 00root root 0000000 0000000 [submodule "MAVProxy/modules/mavproxy_cesium"]
path = MAVProxy/modules/mavproxy_cesium
url = https://github.com/ArduPilot/MAVCesium.git
MAVProxy-1.8.74/ANDROID.md 0000664 0000000 0000000 00000004131 15043245150 0014700 0 ustar 00root root 0000000 0000000 ## install MAVProxy on Android ?
Yes, it's possible to install MAVProxy on Android, totally unsupported, no guarantees, good luck, but here goes.
### install/load Termux Android app from .apk on github
see https://github.com/termux/termux-app for more.
eg: https://github.com/termux/termux-app/releases/download/v0.118.0/termux-app_v0.118.0+github-debug_armeabi-v7a.apk
tip: do NOT install from play store, its a really old version.
Once installed, in the termux console run these commands:
```
pkg upgrade
```
[ answer 'y' to all questions.]
```
pkg install python
```
[3.10.x at time of testing]
```
pkg install libxml2*
```
[ for -dev pkg ]
```
pkg install libxslt*
```
[ for -dev pkg ]
```
pkg install python-*
```
[ for python-dev , be sure to include the '-' here.]
```
pip install --upgrade pip
```
pip is needed for the next few packages
```
pip install wheel
pip install pymavlink
```
wheel isn't installed automatically in some cases, so we install it manually first
#https://superuser.com/questions/1724176/why-pip-throwing-an-error-when-installing-numpy-in-termux
```
MATHLIB="m" pip3 install numpy
```
numpy doesnt install without this extra prefix on android , so we do it first as well.
```
pip install mavproxy
```
finally we install mavproxy!
```
pkg install mlocate
updatedb
```
optionally other things that might be helpful
```
locate mavproxy.py
```
where is it?
```
mavproxy.py
mavproxy.py --master=udp:1.2.3.4:14550
```
a few different ways to run it!
eg: if u have udp mavlink data being forward to your phone over wifi, then 1.2.3.4 should be your PHONE's ip address, and it will receive the data.
known issues:
* --console and all the GUI parts of mavproxy don't work, as the 'WX' package isn't available for android.
* no idea if using a usb/serial connecton works, but UDP-over-wifi definitely does!
* totally unsupported, may break at any time and stop working. please do a pull-request to edit this file if you find these instructions no longer work.
MAVProxy-1.8.74/COPYING.txt 0000664 0000000 0000000 00000105755 15043245150 0015165 0 ustar 00root root 0000000 0000000 GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc.
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
Copyright (C)
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
Copyright (C)
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
.
MAVProxy-1.8.74/MAVProxy/ 0000775 0000000 0000000 00000000000 15043245150 0014764 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/MAVProxy/MAVLink.xml 0000664 0000000 0000000 00000005476 15043245150 0016763 0 ustar 00root root 0000000 0000000
trueailerondouble/controls/flight/aileronelevatordouble/controls/flight/elevatorrudderdouble/controls/flight/rudderthrottledouble/controls/engines/engine[0]/throttle
MAVProxy-1.8.74/MAVProxy/MAVProxyWinLAN.bat 0000664 0000000 0000000 00000000207 15043245150 0020151 0 ustar 00root root 0000000 0000000 cd ..\
python.exe -m pip install --upgrade build . --user
python.exe .\MAVProxy\mavproxy.py --master=0.0.0.0:14550 --console
pause
MAVProxy-1.8.74/MAVProxy/MAVProxyWinUSB.bat 0000664 0000000 0000000 00000000156 15043245150 0020173 0 ustar 00root root 0000000 0000000 cd ..\
python.exe -m pip install --upgrade build . --user
python.exe .\MAVProxy\mavproxy.py --console
pause MAVProxy-1.8.74/MAVProxy/__init__.py 0000664 0000000 0000000 00000000060 15043245150 0017071 0 ustar 00root root 0000000 0000000 '''MAVProxy MAVLink ground station libraries'''
MAVProxy-1.8.74/MAVProxy/mavproxy.py 0000775 0000000 0000000 00000170067 15043245150 0017241 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
mavproxy - a MAVLink proxy program
Copyright Andrew Tridgell 2011
Released under the GNU GPL version 3 or later
AP_FLAKE8_CLEAN
'''
import fnmatch
import glob
import json
import os
import platform
import select
import serial
import shlex
import signal
import socket
import struct
import sys
import threading
import time
import traceback
try:
reload
except NameError:
try:
from importlib import reload
except ImportError:
from imp import reload
from pymavlink import mavutil
from MAVProxy.modules.lib import textconsole
from MAVProxy.modules.lib import mp_util
from MAVProxy.modules.lib import rline
from MAVProxy.modules.lib import mp_module
from MAVProxy.modules.lib import mp_substitute
from MAVProxy.modules.lib import multiproc
from MAVProxy.modules.mavproxy_link import preferred_ports
# adding all this allows pyinstaller to build a working windows executable
# note that using --hidden-import does not work for these modules
try:
multiproc.freeze_support()
from pymavlink import mavwp # noqa
import matplotlib # noqa
import HTMLParser # noqa
except Exception:
pass
# screensaver dbus syntax swiped from
# https://stackoverflow.com/questions/10885337/inhibit-screensaver-with-python
screensaver_interface = None
screensaver_cookie = None
try:
import atexit
import dbus
bus = dbus.SessionBus()
saver = bus.get_object('org.freedesktop.ScreenSaver', '/ScreenSaver')
screensaver_interface = dbus.Interface(saver, dbus_interface='org.freedesktop.ScreenSaver')
if screensaver_cookie is not None:
atexit.register(screensaver_interface.UnInhibit, [screensaver_cookie])
except Exception:
pass
if __name__ == '__main__':
multiproc.freeze_support()
# The MAVLink version being used (None, "1.0", "2.0")
mavversion = None
mpstate = None
class MPStatus(object):
'''hold status information about the mavproxy'''
def __init__(self):
self.gps = None
self.msgs = {}
self.msg_count = {}
self.counters = {'MasterIn' : [], 'MasterOut' : 0, 'FGearIn' : 0, 'FGearOut' : 0, 'Slave' : 0}
self.bytecounters = {'MasterIn': []}
self.setup_mode = opts.setup
self.mav_error = 0
self.altitude = 0
self.last_distance_announce = 0.0
self.exit = False
self.flightmode = 'MAV'
self.last_mode_announce = 0
self.last_mode_announced = 'MAV'
self.logdir = None
self.last_heartbeat = 0
self.last_message = 0
self.heartbeat_error = False
self.last_apm_msg = None
self.last_apm_msg_time = 0
self.statustexts_by_sysidcompid = {}
self.highest_msec = {}
self.have_gps_lock = False
self.lost_gps_lock = False
self.last_gps_lock = 0
self.watch = None
self.watch_verbose = False
self.last_streamrate1 = -1
self.last_streamrate2 = -1
self.last_seq = 0
self.armed = False
self.last_bytecounter_calc = 0
class ByteCounter(object):
def __init__(self):
self.total_count = 0
self.current_count = 0
self.buckets = []
self.max_buckets = 10 # 10 seconds
def update(self, bytecount):
self.total_count += bytecount
self.current_count += bytecount
def rotate(self):
'''move current count into a bucket, zero count'''
# huge assumption made that we're called rapidly enough to
# not need to rotate multiple buckets.
self.buckets.append(self.current_count)
self.current_count = 0
if len(self.buckets) > self.max_buckets:
self.buckets = self.buckets[-self.max_buckets:]
def rate(self):
if len(self.buckets) == 0:
return 0
total = 0
for bucket in self.buckets:
total += bucket
return total/float(len(self.buckets))
def total(self):
return self.total_count
def update_bytecounters(self):
'''rotate bytecounter buckets if required'''
now = time.time()
time_delta = now - self.last_bytecounter_calc
if time_delta < 1:
return
self.last_bytecounter_calc = now
for counter in self.bytecounters['MasterIn']:
counter.rotate()
def show(self, f, pattern=None, verbose=False):
'''write status to status.txt'''
if pattern is None:
f.write('Counters: ')
for c in self.counters:
f.write('%s:%s ' % (c, self.counters[c]))
f.write('\n')
f.write('MAV Errors: %u\n' % self.mav_error)
f.write(str(self.gps)+'\n')
for m in sorted(self.msgs.keys()):
if pattern is not None:
if not fnmatch.fnmatch(str(m).upper(), pattern.upper()):
continue
if getattr(self.msgs[m], '_instance_field', None) is not None and m.find('[') == -1 and pattern.find('*') != -1: # noqa:E501
# only show instance versions for patterns
continue
msg = None
sysid = mpstate.settings.target_system
for mav in mpstate.mav_master:
if sysid not in mav.sysid_state:
continue
if m not in mav.sysid_state[sysid].messages:
continue
msg2 = mav.sysid_state[sysid].messages[m]
if msg is None or msg2._timestamp > msg._timestamp:
msg = msg2
if msg is None:
continue
if verbose:
try:
mavutil.dump_message_verbose(f, msg)
f.write("\n")
except AttributeError as e:
if "has no attribute 'dump_message_verbose'" in str(e):
print("pymavlink update required for --verbose")
else:
raise e
else:
f.write("%u: %s\n" % (self.msg_count[m], str(msg)))
def write(self):
'''write status to status.txt'''
f = open('status.txt', mode='w')
self.show(f)
f.close()
def say_text(text, priority='important'):
'''text output - default function for say()'''
mpstate.console.writeln(text)
def say(text, priority='important'):
'''text and/or speech output'''
mpstate.functions.say(text, priority)
def add_input(cmd, immediate=False):
'''add some command input to be processed'''
if immediate:
process_stdin(cmd)
else:
mpstate.input_queue.put(cmd)
class MAVFunctions(object):
'''core functions available in modules'''
def __init__(self):
self.process_stdin = add_input
self.param_set = param_set
self.get_mav_param = get_mav_param
self.say = say_text
# input handler can be overridden by a module
self.input_handler = None
class MPState(object):
'''holds state of mavproxy'''
def __init__(self):
self.console = textconsole.SimpleConsole()
self.command_map = None
self.map = None
self.map_functions = {}
self.click_location = None
self.click_time = None
self.vehicle_type = None
self.vehicle_name = None
self.aircraft_dir = None
self.logqueue_raw = None
self.logqueue = None
self.rl = None
self.input_queue = None
self.input_count = None
self.empty_input_count = None
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.settings = MPSettings([
MPSetting('link', int, 1, 'Primary Link', tab='Link', range=(0, 100), increment=1),
MPSetting('streamrate', int, 4, 'Stream rate link1', range=(-1, 500), increment=1),
MPSetting('streamrate2', int, 4, 'Stream rate link2', range=(-1, 500), increment=1),
MPSetting('heartbeat', float, 1, 'Heartbeat rate (Hz)', range=(0, 100), increment=0.1),
MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
MPSetting('mavfwd_disarmed', bool, True, 'Allow forwarded control when disarmed'),
MPSetting('mavfwd_rate', bool, False, 'Allow forwarded rate control'),
MPSetting('mavfwd_link', int, -1, 'Forward to a specific link'),
MPSetting('mavfwd_signing', bool, True, 'Sign forwarded messages'),
MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
MPSetting('baudrate', int, opts.baudrate, 'baudrate for new links', range=(0, 10000000), increment=1),
MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
MPSetting('select_timeout', float, 0.01, 'select timeout'),
MPSetting('altreadout', int, 10, 'Altitude Readout',
range=(0, 100), increment=1, tab='Announcements'),
MPSetting('distreadout', int, 200, 'Distance Readout', range=(0, 10000), increment=1),
MPSetting('moddebug', int, opts.moddebug, 'Module Debug Level', range=(0, 4), increment=1, tab='Debug'),
MPSetting('script_fatal', bool, False, 'fatal error on bad script', tab='Debug'),
MPSetting('compdebug', int, 0, 'Computation Debug Mask', range=(0, 3), tab='Debug'),
MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
MPSetting('requireexit', bool, False, 'Require exit command'),
MPSetting('wpupdates', bool, True, 'Announce waypoint updates'),
MPSetting('wpterrainadjust', bool, True, 'Adjust alt of moved wp using terrain'),
MPSetting('wp_use_mission_int', bool, True, 'use MISSION_ITEM_INT messages'),
MPSetting('wp_use_waypoint_set_current', bool, False, 'use deprecated WAYPOINT_SET_CURRENT message'),
MPSetting('basealt', int, 0, 'Base Altitude', range=(0, 30000), increment=1, tab='Altitude'),
MPSetting('wpalt', int, 100, 'Default WP Altitude', range=(0, 10000), increment=1),
MPSetting('rallyalt', int, 90, 'Default Rally Altitude', range=(0, 10000), increment=1),
MPSetting('terrainalt', str, 'Auto', 'Use terrain altitudes', choice=['Auto', 'True', 'False']),
MPSetting('guidedalt', int, 100, 'Default "Fly To" Altitude', range=(0, 10000), increment=1),
MPSetting('guided_use_reposition', bool, True, 'Use MAV_CMD_DO_REPOSITION for guided fly-to'),
MPSetting('rally_breakalt', int, 40, 'Default Rally Break Altitude', range=(0, 10000), increment=1),
MPSetting('rally_flags', int, 0, 'Default Rally Flags', range=(0, 10000), increment=1),
MPSetting('source_system', int, 255, 'MAVLink Source system', range=(0, 255), increment=1, tab='MAVLink'),
MPSetting('source_component', int, 230, 'MAVLink Source component', range=(0, 255), increment=1),
MPSetting('target_system', int, 0, 'MAVLink target system', range=(0, 255), increment=1),
MPSetting('target_component', int, 0, 'MAVLink target component', range=(0, 255), increment=1),
MPSetting('state_basedir', str, None, 'base directory for logs and aircraft directories'),
MPSetting('allow_unsigned', bool, True, 'whether unsigned packets will be accepted'),
MPSetting('dist_unit', str, 'm', 'distance unit', choice=['m', 'nm', 'miles'], tab='Units'),
MPSetting('height_unit', str, 'm', 'height unit', choice=['m', 'feet']),
MPSetting('speed_unit', str, 'm/s', 'height unit', choice=['m/s', 'knots', 'mph']),
MPSetting('flytoframe', str, 'AboveHome', 'frame for FlyTo', choice=['AboveHome', 'AGL', 'AMSL']),
MPSetting('fwdpos', bool, False, 'Forward GLOBAL_POSITION_INT on all links'),
MPSetting('checkdelay', bool, True, 'check for link delay'),
MPSetting('param_ftp', bool, True, 'try ftp for parameter download'),
MPSetting('param_docs', bool, True, 'show help for parameters'),
MPSetting('vehicle_name', str, '', 'Vehicle Name', tab='Vehicle'),
MPSetting('sys_status_error_warn_interval', int, 30, 'interval to warn of autopilot software failure'),
MPSetting('inhibit_screensaver_when_armed', bool, False, 'inhibit screensaver while vehicle armed'),
MPSetting('timeout', int, 5, 'Number of seconds with no packets for a link to considered down', range=(0, 255), increment=1), # noqa
])
self.completions = {
"script" : ["(FILENAME)"],
"set" : ["(SETTING)"],
"status" : ["(VARIABLE)"],
"module" : ["list",
"load (AVAILMODULES)",
" (LOADEDMODULES)"]
}
self.status = MPStatus()
# master mavlink device
self.mav_master = None
# mavlink outputs
self.mav_outputs = []
self.sysid_outputs = {}
# Mapping of all detected sysid's to links
# Key is link id, value is all detected sysid's/compid's in that link
# Dict of self.vehicle_link_map[linknumber] = set([(sysid1,compid1), (sysid2,compid2), ...])
self.vehicle_link_map = {}
# SITL output
self.sitl_output = None
self.mav_param_by_sysid = {}
self.mav_param_by_sysid[(self.settings.target_system, self.settings.target_component)] = mavparm.MAVParmDict()
self.modules = []
self.public_modules = {}
self.functions = MAVFunctions()
self.select_extra = {}
self.continue_mode = False
self.aliases = {}
import platform
self.system = platform.system()
self.multi_instance = {}
self.instance_count = {}
self.is_sitl = False
self.start_time_s = time.time()
self.attitude_time_s = 0
self.position = None
@property
def mav_param(self):
'''map mav_param onto the current target system parameters'''
compid = self.settings.target_component
if compid == 0:
compid = 1
sysid = (self.settings.target_system, compid)
if sysid not in self.mav_param_by_sysid:
self.mav_param_by_sysid[sysid] = mavparm.MAVParmDict()
return self.mav_param_by_sysid[sysid]
def module(self, name):
'''Find a public module (most modules are private)'''
if name in self.public_modules:
return self.public_modules[name]
return None
def load_module(self, modname, quiet=False, **kwargs):
'''load a module'''
modpaths = ['MAVProxy.modules.mavproxy_%s' % modname, modname]
for (m, pm) in mpstate.modules:
if m.name == modname and modname not in mpstate.multi_instance:
if not quiet:
print("module %s already loaded" % modname)
# don't report an error
return True
ex = None
for modpath in modpaths:
try:
m = import_package(modpath)
reload(m)
module = m.init(mpstate, **kwargs)
if isinstance(module, mp_module.MPModule):
mpstate.modules.append((module, m))
if not quiet:
if kwargs:
print("Loaded module %s with kwargs = %s" % (modname, kwargs))
else:
print("Loaded module %s" % (modname,))
return True
else:
ex = "%s.init did not return a MPModule instance" % modname
break
except ImportError as msg:
ex = msg
if mpstate.settings.moddebug > 1:
print(get_exception_stacktrace(ex))
help_traceback = ""
if mpstate.settings.moddebug < 3:
help_traceback = " Use 'set moddebug 3' in the MAVProxy console to enable traceback"
print("Failed to load module: %s.%s" % (ex, help_traceback))
return False
def unload_module(self, modname):
'''unload a module'''
for (m, pm) in mpstate.modules:
if m.name == modname:
if hasattr(m, 'unload'):
t = threading.Thread(target=lambda : m.unload(), name="unload %s" % modname)
t.start()
t.join(timeout=5)
if t.is_alive():
print("unload on module %s did not complete" % m.name)
mpstate.modules.remove((m, pm))
return False
mpstate.modules.remove((m, pm))
if modname in mpstate.public_modules:
del mpstate.public_modules[modname]
print("Unloaded module %s" % modname)
return True
print("Unable to find module %s" % modname)
return False
def master(self, target_sysid=-1):
'''return the currently chosen mavlink master object'''
if len(self.mav_master) == 0:
return None
if self.settings.link > len(self.mav_master):
self.settings.link = 1
if target_sysid != -1:
# if we're looking for a specific system ID then try to find best
# link for that
best_link = None
best_timestamp = 0
for m in self.mav_master:
try:
tstamp = m.sysid_state[target_sysid].messages['HEARTBEAT']._timestamp
except Exception:
continue
if tstamp > best_timestamp:
best_link = m
best_timestamp = tstamp
if best_link is not None:
return best_link
# try to use one with no link error
if not self.mav_master[self.settings.link-1].linkerror:
return self.mav_master[self.settings.link-1]
for m in self.mav_master:
if not m.linkerror:
return m
return self.mav_master[self.settings.link-1]
def foreach_mav(self, sysid, compid, closure):
# Send mavlink message only on all links that contain vehicle (sysid, compid)
# More efficient than just blasting all links, when sending targetted messages
# Also useful for sending messages to non-selected vehicles
for linkNumber, vehicleList in self.vehicle_link_map.items():
if (sysid, compid) not in vehicleList:
continue
closure(self.mav_master[linkNumber].mav)
def notify_click(self):
notify_mods = ['map', 'misseditor']
for modname in notify_mods:
mod = self.module(modname)
if mod is not None:
mod.click_updated()
def click(self, latlng):
if latlng is None:
self.click_location = None
self.click_time = None
self.notify_click()
return
(lat, lng) = latlng
if lat is None:
print("Bad Lat")
return
if lng is None:
print("Bad lng")
return
self.click_location = (lat, lng)
self.click_time = time.time()
self.notify_click()
def get_mav_param(param, default=None):
'''return a EEPROM parameter value'''
return mpstate.mav_param.get(param, default)
def param_set(name, value, retries=3):
'''set a parameter'''
name = name.upper()
return mpstate.mav_param.mavset(mpstate.master(), name, value, retries=retries)
def cmd_script(args):
'''run a script'''
if len(args) < 1:
print("usage: script ")
return
run_script(args[0])
def cmd_set(args):
'''control mavproxy options'''
mpstate.settings.command(args)
def cmd_status(args):
'''show status'''
verbose = False
if "--verbose" in args:
verbose = True
args = list(filter(lambda x : x != "--verbose", args))
if len(args) == 0:
mpstate.status.show(sys.stdout, pattern=None, verbose=verbose)
else:
for pattern in args:
mpstate.status.show(sys.stdout, pattern=pattern, verbose=verbose)
def cmd_setup(args):
mpstate.status.setup_mode = True
mpstate.rl.set_prompt("")
def cmd_reset(args):
print("Resetting master")
mpstate.master().reset()
def cmd_click(args):
'''synthesise click at lat/lon; no arguments is "unclick"'''
usage = "click "
if len(args) == 0:
mpstate.click(None)
return
if len(args) < 1:
print(usage)
return
if args[0] == "show":
print("%.8f %.8f" % mpstate.click_location)
return
if len(args) < 2:
print(usage)
return
lat = mavutil.evaluate_expression(args[0], mpstate.master().messages)
lng = mavutil.evaluate_expression(args[1], mpstate.master().messages)
mpstate.click((lat, lng))
def cmd_watch(args):
'''watch a mavlink packet pattern'''
if len(args) == 0:
mpstate.status.watch = None
mpstate.status.watch_verbose = False
return
if "--verbose" in args:
mpstate.status.watch_verbose = True
args.remove('--verbose')
else:
mpstate.status.watch_verbose = False
mpstate.status.watch = args
print("Watching %s" % mpstate.status.watch)
def generate_kwargs(args):
kwargs = {}
module_components = args.split(":{", 1)
module_name = module_components[0]
if (len(module_components) == 2 and module_components[1].endswith("}")):
# assume json
try:
module_args = "{"+module_components[1]
kwargs = json.loads(module_args)
except ValueError as e:
print('Invalid JSON argument: {0} ({1})'.format(module_args,
repr(e)))
return (module_name, kwargs)
def get_exception_stacktrace(e):
if sys.version_info[0] >= 3:
ret = "%s\n" % e
ret += ''.join(traceback.format_exception(type(e),
e,
tb=e.__traceback__))
return ret
return traceback.format_exc(e)
def cmd_module(args):
'''module commands'''
usage = "usage: module "
if len(args) < 1:
print(usage)
return
if args[0] == "list":
mods = []
for (m, pm) in mpstate.modules:
mods.append(m)
mods = sorted(mods, key=lambda m : m.name)
for m in mods:
print("%s: %s" % (m.name, m.description))
elif args[0] == "load":
if len(args) < 2:
print("usage: module load ")
return
(modname, kwargs) = generate_kwargs(args[1])
try:
mpstate.load_module(modname, **kwargs)
except TypeError as ex:
print(ex)
print("%s module does not support keyword arguments" % modname)
return
elif args[0] == "reload":
if len(args) < 2:
print("usage: module reload ")
return
(modname, kwargs) = generate_kwargs(args[1])
pmodule = None
for (m, pm) in mpstate.modules:
if m.name == modname:
pmodule = pm
if pmodule is None:
print("Module %s not loaded" % modname)
return
if mpstate.unload_module(modname):
try:
reload(pmodule)
except ImportError:
clear_zipimport_cache()
reload(pmodule)
try:
if mpstate.load_module(modname, quiet=True, **kwargs):
print("Reloaded module %s" % modname)
except TypeError:
print("%s module does not support keyword arguments" % modname)
elif args[0] == "unload":
if len(args) < 2:
print("usage: module unload ")
return
modname = os.path.basename(args[1])
mpstate.unload_module(modname)
else:
print(usage)
def cmd_alias(args):
'''alias commands'''
usage = "usage: alias "
if len(args) < 1 or args[0] == "list":
if len(args) >= 2:
wildcard = args[1].upper()
else:
wildcard = '*'
for a in sorted(mpstate.aliases.keys()):
if fnmatch.fnmatch(a.upper(), wildcard):
print("%-15s : %s" % (a, mpstate.aliases[a]))
elif args[0] == "add":
if len(args) < 3:
print(usage)
return
a = args[1]
mpstate.aliases[a] = ' '.join(args[2:])
elif args[0] == "remove":
if len(args) != 2:
print(usage)
return
a = args[1]
if a in mpstate.aliases:
mpstate.aliases.pop(a)
else:
print("no alias %s" % a)
else:
print(usage)
return
def clear_zipimport_cache():
"""Clear out cached entries from _zip_directory_cache.
See http://www.digi.com/wiki/developer/index.php/Error_messages"""
import sys
import zipimport
syspath_backup = list(sys.path)
zipimport._zip_directory_cache.clear()
# load back items onto sys.path
sys.path = syspath_backup
# add this too: see https://mail.python.org/pipermail/python-list/2005-May/353229.html
sys.path_importer_cache.clear()
# http://stackoverflow.com/questions/211100/pythons-import-doesnt-work-as-expected
# has info on why this is necessary.
def import_package(name):
"""Given a package name like 'foo.bar.quux', imports the package
and returns the desired module."""
try:
mod = __import__(name)
except ImportError:
clear_zipimport_cache()
mod = __import__(name)
components = name.split('.')
for comp in components[1:]:
mod = getattr(mod, comp)
return mod
command_map = {
'script' : (cmd_script, 'run a script of MAVProxy commands'), # noqa:E241
'setup' : (cmd_setup, 'go into setup mode'), # noqa:E241
'reset' : (cmd_reset, 'reopen the connection to the MAVLink master'), # noqa:E241
'click' : (cmd_click, 'set click location'), # noqa:E241
'status' : (cmd_status, 'show status'), # noqa:E241
'set' : (cmd_set, 'mavproxy settings'), # noqa:E241
'watch' : (cmd_watch, 'watch a MAVLink pattern'), # noqa:E241
'module' : (cmd_module, 'module commands'), # noqa:E241
'alias' : (cmd_alias, 'command aliases') # noqa:E241
}
def shlex_quotes(value):
'''see http://stackoverflow.com/questions/6868382/python-shlex-split-ignore-single-quotes'''
lex = shlex.shlex(value)
lex.quotes = '"'
lex.whitespace_split = True
lex.commenters = ''
return list(lex)
def process_stdin(line):
'''handle commands from user'''
if line is None:
sys.exit(0)
# allow for modules to override input handling
if mpstate.functions.input_handler is not None:
mpstate.functions.input_handler(line)
return
line = line.strip()
if mpstate.status.setup_mode:
# in setup mode we send strings straight to the master
if line == '.':
mpstate.status.setup_mode = False
mpstate.status.flightmode = "MAV"
mpstate.rl.set_prompt("MAV> ")
return
if line != '+++':
line += '\r'
for c in line:
time.sleep(0.01)
if sys.version_info.major >= 3:
mpstate.master().write(bytes(c, "ascii"))
else:
mpstate.master().write(c)
return
if not line:
return
try:
args = shlex_quotes(line)
except Exception as e:
print("Caught shlex exception: %s" % str(e))
return
# strip surrounding quotes - shlex leaves them in place
new_args = []
for arg in args:
new_arg = arg
for q in "'", '"':
if arg.startswith(q) and arg.endswith(q):
new_arg = arg[1:-1]
break
new_args.append(new_arg)
args = new_args
cmd = args[0]
while cmd in mpstate.aliases:
line = mpstate.aliases[cmd]
args = shlex.split(line) + args[1:]
cmd = args[0]
if cmd == 'help':
k = command_map.keys()
k = sorted(k)
for cmd in k:
(fn, help) = command_map[cmd]
print("%-15s : %s" % (cmd, help))
return
if cmd == 'exit' and mpstate.settings.requireexit:
mpstate.status.exit = True
return
if cmd not in command_map:
for (m, pm) in mpstate.modules:
if hasattr(m, 'unknown_command'):
try:
if m.unknown_command(args):
return
except Exception as e:
print("ERROR in command: %s" % str(e))
print("Unknown command '%s'" % line)
return
(fn, help) = command_map[cmd]
try:
fn(args[1:])
except Exception as e:
print("ERROR in command %s: %s" % (args[1:], str(e)))
if mpstate.settings.moddebug > 1:
traceback.print_exc()
def process_master(m):
'''process packets from the MAVLink master'''
try:
s = m.recv(16*1024)
except Exception:
time.sleep(0.1)
return
# prevent a dead serial port from causing the CPU to spin. The user hitting enter will
# cause it to try and reconnect
if len(s) == 0:
time.sleep(0.1)
return
mpstate.status.bytecounters['MasterIn'][m.linknum].update(len(s))
if (mpstate.settings.compdebug & 1) != 0:
return
if mpstate.logqueue_raw:
mpstate.logqueue_raw.put(bytearray(s))
if mpstate.status.setup_mode:
if mpstate.system == 'Windows':
# strip nsh ansi codes
s = s.replace("\033[K", "")
if sys.version_info.major >= 3:
sys.stdout.write(str(s, "ascii", "ignore"))
else:
sys.stdout.write(str(s))
sys.stdout.flush()
return
if m.first_byte and mavversion is None:
m.auto_mavlink_version(s)
msgs = m.mav.parse_buffer(s)
if msgs:
for msg in msgs:
sysid = msg.get_srcSystem()
if sysid in mpstate.sysid_outputs:
# the message has been handled by a specialised
# handler for this system
continue
if getattr(m, '_timestamp', None) is None:
m.post_message(msg)
if msg.get_type() == "BAD_DATA":
if opts.show_errors:
mpstate.console.writeln("MAV error: %s" % msg)
mpstate.status.mav_error += 1
def process_mavlink(slave):
'''process packets from MAVLink slaves, forwarding to the master'''
try:
buf = slave.recv()
except socket.error:
return
try:
if slave.first_byte and mavversion is None:
slave.auto_mavlink_version(buf)
msgs = slave.mav.parse_buffer(buf)
except mavutil.mavlink.MAVError as e:
mpstate.console.error("Bad MAVLink slave message from %s: %s" % (slave.address, e.message))
return
if msgs is None:
return
allow_fwd = mpstate.settings.mavfwd
if not allow_fwd and mpstate.settings.mavfwd_disarmed and not mpstate.master(-1).motors_armed():
allow_fwd = True
if mpstate.status.setup_mode:
allow_fwd = False
if allow_fwd:
for m in msgs:
target_sysid = getattr(m, 'target_system', -1)
if mpstate.settings.mavfwd_link > 0 and mpstate.settings.mavfwd_link <= len(mpstate.mav_master):
output = mpstate.mav_master[mpstate.settings.mavfwd_link-1]
else:
# find best link by sysid
output = mpstate.master(target_sysid)
if (mpstate.settings.mavfwd_signing and
output.mav.signing.sign_outgoing and
(m._header.incompat_flags & mavutil.mavlink.MAVLINK_IFLAG_SIGNED) == 0):
# repack the message if this is a signed link and not already signed
m.pack(output.mav)
output.write(m.get_msgbuf())
if mpstate.logqueue:
usec = int(time.time() * 1.0e6)
mpstate.logqueue.put(bytearray(struct.pack('>Q', usec) + m.get_msgbuf()))
if mpstate.status.watch:
for msg_type in mpstate.status.watch:
if fnmatch.fnmatch(m.get_type().upper(), msg_type.upper()):
mpstate.console.writeln('> ' + str(m))
break
mpstate.status.counters['Slave'] += 1
def mkdir_p(dir):
'''like mkdir -p'''
if not dir:
return
if dir.endswith("/"):
mkdir_p(dir[:-1])
return
if os.path.isdir(dir):
return
mkdir_p(os.path.dirname(dir))
os.mkdir(dir)
def log_writer():
'''log writing thread'''
while not mpstate.status.exit:
mpstate.logfile_raw.write(bytearray(mpstate.logqueue_raw.get()))
timeout = time.time() + 10
while not mpstate.logqueue_raw.empty() and time.time() < timeout:
mpstate.logfile_raw.write(mpstate.logqueue_raw.get())
while not mpstate.logqueue.empty() and time.time() < timeout:
mpstate.logfile.write(mpstate.logqueue.get())
if mpstate.settings.flushlogs or time.time() >= timeout:
mpstate.logfile.flush()
mpstate.logfile_raw.flush()
# If state_basedir is NOT set then paths for logs and aircraft
# directories are relative to mavproxy's cwd
def log_paths():
'''Returns tuple (logdir, telemetry_log_filepath, raw_telemetry_log_filepath)'''
if opts.aircraft is not None:
dirname = ""
if opts.mission is not None:
print(opts.mission)
dirname += "%s/logs/%s/Mission%s" % (opts.aircraft, time.strftime("%Y-%m-%d"), opts.mission)
else:
dirname += "%s/logs/%s" % (opts.aircraft, time.strftime("%Y-%m-%d"))
# dirname is currently relative. Possibly add state_basedir:
if mpstate.settings.state_basedir is not None:
dirname = os.path.join(mpstate.settings.state_basedir, dirname)
mkdir_p(dirname)
highest = None
for i in range(1, 10000):
fdir = os.path.join(dirname, 'flight%u' % i)
if not os.path.exists(fdir):
break
highest = fdir
if mpstate.continue_mode and highest is not None:
fdir = highest
elif os.path.exists(fdir):
print("Flight logs full")
sys.exit(1)
logname = 'flight.tlog'
logdir = fdir
else:
logname = os.path.basename(opts.logfile)
dir_path = os.path.dirname(opts.logfile)
if not os.path.isabs(dir_path) and mpstate.settings.state_basedir is not None:
dir_path = os.path.join(mpstate.settings.state_basedir, dir_path)
logdir = dir_path
mkdir_p(logdir)
return (logdir,
os.path.join(logdir, logname),
os.path.join(logdir, logname + '.raw'))
def open_telemetry_logs(logpath_telem, logpath_telem_raw):
'''open log files'''
if opts.append_log or opts.continue_mode:
mode = 'ab'
else:
mode = 'wb'
try:
mpstate.logfile = open(logpath_telem, mode=mode)
mpstate.logfile_raw = open(logpath_telem_raw, mode=mode)
print("Log Directory: %s" % mpstate.status.logdir)
print("Telemetry log: %s" % logpath_telem)
# make sure there's enough free disk space for the logfile (>200Mb)
# statvfs doesn't work in Windows
if platform.system() != 'Windows':
stat = os.statvfs(logpath_telem)
if stat.f_bfree*stat.f_bsize < 209715200:
print("ERROR: Not enough free disk space for logfile")
mpstate.status.exit = True
return
# use a separate thread for writing to the logfile to prevent
# delays during disk writes (important as delays can be long if camera
# app is running)
t = threading.Thread(target=log_writer, name='log_writer')
t.daemon = True
t.start()
except Exception as e:
print("ERROR: opening log file for writing: %s" % e)
mpstate.status.exit = True
return
def set_stream_rates():
'''set mavlink stream rates'''
if (not msg_period.trigger() and
mpstate.status.last_streamrate1 == mpstate.settings.streamrate and
mpstate.status.last_streamrate2 == mpstate.settings.streamrate2):
return
mpstate.status.last_streamrate1 = mpstate.settings.streamrate
mpstate.status.last_streamrate2 = mpstate.settings.streamrate2
for master in mpstate.mav_master:
if master.linknum == 0:
rate = mpstate.settings.streamrate
else:
rate = mpstate.settings.streamrate2
if rate != -1 and mpstate.settings.streamrate != -1:
# Send to all detected vehicles in this link
for (sysid, compid) in mpstate.vehicle_link_map[master.linknum]:
master.mav.request_data_stream_send(sysid, compid,
mavutil.mavlink.MAV_DATA_STREAM_ALL,
rate, 1)
def check_link_status():
'''check status of master links'''
tnow = time.time()
if mpstate.status.last_message != 0 and tnow > mpstate.status.last_message + mpstate.settings.timeout:
say("no link")
mpstate.status.heartbeat_error = True
for master in mpstate.mav_master:
if not master.linkerror and (tnow > master.last_message + mpstate.settings.timeout or master.portdead):
say("link %s down" % (mp_module.MPModule.link_label(master)))
master.linkerror = True
def send_heartbeat(master):
if master.mavlink10():
master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID,
0, 0, 0)
else:
MAV_GROUND = 5
MAV_AUTOPILOT_NONE = 4
master.mav.heartbeat_send(MAV_GROUND, MAV_AUTOPILOT_NONE)
def periodic_tasks():
'''run periodic checks'''
if mpstate.status.setup_mode:
return
if (mpstate.settings.compdebug & 2) != 0:
return
if mpstate.settings.heartbeat != 0:
heartbeat_period.frequency = mpstate.settings.heartbeat
if heartbeat_period.trigger() and mpstate.settings.heartbeat != 0:
mpstate.status.counters['MasterOut'] += 1
for master in mpstate.mav_master:
send_heartbeat(master)
if heartbeat_check_period.trigger():
check_link_status()
set_stream_rates()
mpstate.status.update_bytecounters()
# call optional module idle tasks. These are called at several hundred Hz
for (m, pm) in mpstate.modules:
if hasattr(m, 'idle_task'):
try:
m.idle_task()
except Exception as msg:
if mpstate.settings.moddebug == 1:
print(msg)
elif mpstate.settings.moddebug > 1:
print(get_exception_stacktrace(msg))
# also see if the module should be unloaded:
if m.needs_unloading:
mpstate.unload_module(m.name)
def main_loop():
'''main processing loop'''
global screensaver_cookie
if not mpstate.status.setup_mode and not opts.nowait:
for master in mpstate.mav_master:
if master.linknum != 0:
break
print("Waiting for heartbeat from %s" % master.address)
send_heartbeat(master)
master.wait_heartbeat(timeout=0.1)
set_stream_rates()
while True:
if mpstate is None or mpstate.status.exit:
return
# enable or disable screensaver:
if (mpstate.settings.inhibit_screensaver_when_armed and
screensaver_interface is not None):
if mpstate.status.armed and screensaver_cookie is None:
# now we can inhibit the screensaver
screensaver_cookie = screensaver_interface.Inhibit("MAVProxy",
"Vehicle is armed")
elif not mpstate.status.armed and screensaver_cookie is not None:
# we can also restore it
screensaver_interface.UnInhibit(screensaver_cookie)
screensaver_cookie = None
while not mpstate.input_queue.empty():
line = mpstate.input_queue.get()
mpstate.input_count += 1
cmds = line.split(';')
if len(cmds) == 1 and cmds[0] == "":
mpstate.empty_input_count += 1
for c in cmds:
process_stdin(c)
for master in mpstate.mav_master:
if master.fd is None:
try:
if master.port.inWaiting() > 0:
process_master(master)
except serial.SerialException:
pass
periodic_tasks()
rin = []
for master in mpstate.mav_master:
if master.fd is not None and not master.portdead:
rin.append(master.fd)
for m in mpstate.mav_outputs:
if m.fd is not None:
rin.append(m.fd)
for sysid in mpstate.sysid_outputs:
m = mpstate.sysid_outputs[sysid]
if m.fd is not None:
rin.append(m.fd)
if rin == []:
time.sleep(0.0001)
continue
for fd in mpstate.select_extra:
if fd is not None:
rin.append(fd)
try:
(rin, win, xin) = select.select(rin, [], [], mpstate.settings.select_timeout)
except select.error:
continue
if mpstate is None:
return
for fd in rin:
if mpstate is None:
return
for master in mpstate.mav_master:
if fd == master.fd:
process_master(master)
if mpstate is None:
return
continue
for m in mpstate.mav_outputs:
if fd == m.fd:
process_mavlink(m)
if mpstate is None:
return
continue
for sysid in mpstate.sysid_outputs:
m = mpstate.sysid_outputs[sysid]
if fd == m.fd:
process_mavlink(m)
if mpstate is None:
return
continue
# this allow modules to register their own file descriptors
# for the main select loop
if fd in mpstate.select_extra:
try:
# call the registered read function
(fn, args) = mpstate.select_extra[fd]
fn(args)
except Exception as msg:
if mpstate.settings.moddebug == 1:
print(msg)
# on an exception, remove it from the select list
mpstate.select_extra.pop(fd)
def input_loop():
'''wait for user input'''
while mpstate.status.exit is not True:
try:
line = mpstate.rl.input()
mpstate.input_queue.put(line)
except (EOFError, IOError):
mpstate.status.exit = True
def run_script(scriptfile):
'''run a script file'''
try:
f = open(scriptfile, mode='r')
except Exception:
return
mpstate.console.writeln("Running script %s" % scriptfile)
sub = mp_substitute.MAVSubstitute()
for line in f:
line = line.strip()
if line == "" or line.startswith('#'):
continue
try:
line = sub.substitute(line, os.environ)
except mp_substitute.MAVSubstituteError as ex:
print("Bad variable: %s" % str(ex))
if mpstate.settings.script_fatal:
sys.exit(1)
continue
if line.startswith('@'):
line = line[1:]
else:
mpstate.console.writeln("-> %s" % line)
process_stdin(line)
f.close()
def set_mav_version(mav10, mav20, autoProtocol, mavversionArg):
'''Set the Mavlink version based on commandline options'''
# if(mav10 == True or mav20 == True or autoProtocol == True):
# print("Warning: Using deprecated --mav10, --mav20 or --auto-protocol options. Use --mavversion instead")
# sanity check the options
if (mav10 or mav20) and autoProtocol:
print("Error: Can't have [--mav10, --mav20] and --auto-protocol both True")
sys.exit(1)
if mav10 and mav20:
print("Error: Can't have --mav10 and --mav20 both True")
sys.exit(1)
if mavversionArg is not None and (mav10 or mav20 or autoProtocol):
print("Error: Can't use --mavversion with legacy (--mav10, --mav20 or --auto-protocol) options")
sys.exit(1)
# and set the specific mavlink version (False = autodetect)
global mavversion
if mavversionArg == "1.0" or mav10:
os.environ['MAVLINK09'] = '1'
mavversion = "1"
else:
os.environ['MAVLINK20'] = '1'
mavversion = "2"
def run_startup_scripts():
start_scripts = []
if not opts.setup:
if 'HOME' in os.environ:
start_scripts.append(os.path.join(os.environ['HOME'], ".mavinit.scr"))
start_script = mp_util.dot_mavproxy("mavinit.scr")
start_scripts.append(start_script)
if (mpstate.settings.state_basedir is not None and
opts.aircraft is not None):
start_script = os.path.join(mpstate.aircraft_dir, "mavinit.scr")
start_scripts.append(start_script)
for start_script in start_scripts:
if os.path.exists(start_script):
print("Running script (%s)" % (start_script))
run_script(start_script)
if opts.aircraft is not None:
start_script = os.path.join(opts.aircraft, "mavinit.scr")
if os.path.exists(start_script):
run_script(start_script)
else:
print("no script %s" % start_script)
if __name__ == '__main__':
from optparse import OptionParser
parser = OptionParser("mavproxy.py [options]")
parser.add_option("--master", dest="master", action='append',
metavar="DEVICE[,BAUD]", help="MAVLink master port and optional baud rate",
default=[])
parser.add_option("", "--force-connected", dest="force_connected", help="Use master even if initial connection fails",
action='store_true', default=False)
parser.add_option("--out", dest="output", action='append',
metavar="DEVICE[,BAUD]", help="MAVLink output port and optional baud rate",
default=[])
parser.add_option("--baudrate", dest="baudrate", type='int',
help="default serial baud rate", default=57600)
parser.add_option("--sitl", dest="sitl", default=None, help="SITL output port")
parser.add_option("--streamrate", dest="streamrate", default=4, type='int',
help="MAVLink stream rate")
parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
default=255, help='MAVLink source system for this GCS')
parser.add_option("--source-component", dest='SOURCE_COMPONENT', type='int',
default=230, help='MAVLink source component for this GCS')
parser.add_option("--target-system", dest='TARGET_SYSTEM', type='int',
default=0, help='MAVLink target master system')
parser.add_option("--target-component", dest='TARGET_COMPONENT', type='int',
default=0, help='MAVLink target master component')
parser.add_option("--logfile", dest="logfile", help="MAVLink master logfile",
default='mav.tlog')
parser.add_option("-a", "--append-log", dest="append_log", help="Append to log files",
action='store_true', default=False)
parser.add_option("--quadcopter", dest="quadcopter", help="use quadcopter controls",
action='store_true', default=False)
parser.add_option("--setup", dest="setup", help="start in setup mode",
action='store_true', default=False)
parser.add_option("--nodtr", dest="nodtr", help="disable DTR drop on close",
action='store_true', default=False)
parser.add_option("--show-errors", dest="show_errors", help="show MAVLink error packets",
action='store_true', default=False)
parser.add_option("--speech", dest="speech", help="use text to speech",
action='store_true', default=False)
parser.add_option("--aircraft", dest="aircraft", help="aircraft name", default=None)
parser.add_option("--cmd", dest="cmd", help="initial commands", default=None, action='append')
parser.add_option("--console", action='store_true', help="use GUI console")
parser.add_option("--heartbeat-rate", dest="heartbeat", default=1, type='float',
help="MAVLink HEARTBEAT rate")
if platform.system() == 'Windows':
parser.add_option("--no-console", action='store_true', help="don't use GUI console")
parser.add_option("--map", action='store_true', help="load map module")
parser.add_option(
'--load-module',
action='append',
default=[],
help='Load the specified module. Can be used multiple times, or with a comma separated list')
parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
parser.add_option("--mav20", action='store_true', default=False, help="Use MAVLink protocol 2.0")
parser.add_option("--auto-protocol", action='store_true', default=False, help="Auto detect MAVLink protocol version")
parser.add_option("--mavversion", type='choice', choices=['1.0', '2.0'] , help="Force MAVLink Version (1.0, 2.0). Otherwise autodetect version") # noqa:E501
parser.add_option("--nowait", action='store_true', default=False, help="don't wait for HEARTBEAT on startup")
parser.add_option("-c", "--continue", dest='continue_mode', action='store_true', default=False, help="continue logs")
parser.add_option("--dialect", default="all", help="MAVLink dialect")
parser.add_option("--rtscts", action='store_true', help="enable hardware RTS/CTS flow control")
parser.add_option("--moddebug", type=int, help="module debug level", default=0)
parser.add_option("--mission", dest="mission", help="mission name", default=None)
parser.add_option("--daemon", action='store_true', help="run in daemon mode, do not start interactive shell")
parser.add_option("--non-interactive", action='store_true', help="do not start interactive shell")
parser.add_option("--profile", action='store_true', help="run the Yappi python profiler")
parser.add_option("--state-basedir", default=None, help="base directory for logs and aircraft directories")
parser.add_option("--no-state", action='store_true', default=False, help="Don't save logs and other state to disk. Useful for read-only filesystems or long-running systems.") # noqa:E501
parser.add_option("--version", action='store_true', help="version information")
parser.add_option("--default-modules", default="log,signing,wp,rally,fence,ftp,param,relay,tuneopt,arm,mode,calibration,rc,auxopt,misc,cmdlong,battery,terrain,output,adsb,layout", help='default module list') # noqa:E501
parser.add_option("--udp-timeout", dest="udp_timeout", default=0.0, type='float', help="Timeout for udp clients in seconds") # noqa:E501
parser.add_option("--retries", type=int, help="number of times to retry connection", default=3)
(opts, args) = parser.parse_args()
if len(args) != 0:
print("ERROR: mavproxy takes no position arguments; got (%s)" % str(args))
sys.exit(1)
# warn people about ModemManager which interferes badly with APM and Pixhawk
if os.path.exists("/usr/sbin/ModemManager"):
print("WARNING: You should uninstall ModemManager as it conflicts with APM and Pixhawk")
# set the Mavlink version, if required
set_mav_version(opts.mav10, opts.mav20, opts.auto_protocol, opts.mavversion)
from pymavlink import mavparm
mavutil.set_dialect(opts.dialect)
# version information
if opts.version:
# pkg_resources doesn't work in the windows exe build, so read the version file
try:
import pkg_resources
version = pkg_resources.require("mavproxy")[0].version
except Exception:
start_script = mp_util.dot_mavproxy("version.txt")
f = open(start_script, 'r')
version = f.readline()
print("MAVProxy is a modular ground station using the mavlink protocol")
print("MAVProxy Version: " + version)
sys.exit(0)
# global mavproxy state
mpstate = MPState()
mpstate.status.exit = False
mpstate.command_map = command_map
mpstate.continue_mode = opts.continue_mode
# queues for logging
if not opts.no_state:
mpstate.logqueue = multiproc.Queue()
mpstate.logqueue_raw = multiproc.Queue()
else:
mpstate.logqueue = None
mpstate.logqueue_raw = None
if opts.speech:
# start the speech-dispatcher early, so it doesn't inherit any ports from
# modules/mavutil
mpstate.load_module('speech')
serial_list = mavutil.auto_detect_serial(preferred_list=preferred_ports)
serial_list.sort(key=lambda x: x.device)
# remove OTG2 ports for dual CDC
if len(serial_list) == 2 and serial_list[0].device.startswith("/dev/serial/by-id"):
if serial_list[0].device[:-1] == serial_list[1].device[0:-1]:
serial_list.pop(1)
if not opts.master:
print('Auto-detected serial ports are:')
for port in serial_list:
print("%s" % port)
# container for status information
mpstate.settings.target_system = opts.TARGET_SYSTEM
mpstate.settings.target_component = opts.TARGET_COMPONENT
mpstate.mav_master = []
mpstate.rl = rline.rline("MAV> ", mpstate)
def quit_handler(signum=None, frame=None):
# print('Signal handler called with signal', signum)
if mpstate.status.exit:
print('Clean shutdown impossible, forcing an exit')
sys.exit(0)
else:
mpstate.status.exit = True
# Listen for kill signals to cleanly shutdown modules
fatalsignals = [signal.SIGTERM]
try:
fatalsignals.append(signal.SIGQUIT)
signal.signal(signal.SIGHUP, signal.SIG_IGN)
except Exception:
pass
if opts.daemon or opts.non_interactive: # SIGINT breaks readline parsing - if we are interactive, just let things die
fatalsignals.append(signal.SIGINT)
for sig in fatalsignals:
signal.signal(sig, quit_handler)
mpstate.load_module('link', quiet=True)
# load signing early to allow for use of ~/.mavproxy/signing.keys
standard_modules = opts.default_modules.split(',')
if 'signing' in standard_modules:
mpstate.load_module('signing', quiet=True)
mpstate.settings.source_system = opts.SOURCE_SYSTEM
mpstate.settings.source_component = opts.SOURCE_COMPONENT
# open master link
for mdev in opts.master:
if mdev.find('?') != -1 or mdev.find('*') != -1:
for m in glob.glob(mdev):
if not mpstate.module('link').link_add(m, force_connected=opts.force_connected, retries=opts.retries):
sys.exit(1)
elif not mpstate.module('link').link_add(mdev, force_connected=opts.force_connected, retries=opts.retries):
sys.exit(1)
if not opts.master and len(serial_list) == 1:
print("Connecting to %s" % serial_list[0])
mpstate.module('link').link_add(serial_list[0].device)
elif not opts.master and len(serial_list) > 1:
print("Warning: multiple possible serial ports. Use console GUI or 'link add' to add port, or restart using --master to select a single port") # noqa:E501
# if no display, assume running CLI mode and exit
if platform.system() != 'Windows' and "DISPLAY" not in os.environ:
sys.exit(1)
elif not opts.master:
wifi_device = '0.0.0.0:14550'
mpstate.module('link').link_add(wifi_device)
# open any mavlink output ports
for port in opts.output:
# cope with older pymavlink
if opts.udp_timeout > 0:
mpstate.mav_outputs.append(mavutil.mavlink_connection(
port,
baud=int(opts.baudrate),
input=False,
udp_timeout=opts.udp_timeout,
autoreconnect=True
))
else:
mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(opts.baudrate),
input=False, autoreconnect=True))
if opts.sitl:
mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
mpstate.settings.streamrate = opts.streamrate
mpstate.settings.streamrate2 = opts.streamrate
mpstate.settings.heartbeat = opts.heartbeat
if opts.state_basedir is not None:
mpstate.settings.state_basedir = opts.state_basedir
msg_period = mavutil.periodic_event(1.0/15)
heartbeat_period = mavutil.periodic_event(1)
heartbeat_check_period = mavutil.periodic_event(0.33)
mpstate.input_queue = multiproc.Queue()
mpstate.input_count = 0
mpstate.empty_input_count = 0
if opts.setup:
mpstate.rl.set_prompt("")
# call this early so that logdir is setup based on --aircraft
if not opts.no_state:
(mpstate.status.logdir, logpath_telem, logpath_telem_raw) = log_paths()
for module in opts.load_module:
modlist = module.split(',')
for mod in modlist:
process_stdin('module load %s' % (mod))
if not opts.setup:
# some core functionality is in modules
for m in standard_modules:
if m:
mpstate.load_module(m, quiet=True)
if platform.system() != 'Windows':
if opts.console:
process_stdin('module load console')
else:
# default to having console on windows
if opts.console or not opts.no_console:
process_stdin('module load console')
if opts.map:
process_stdin('module load map')
if (mpstate.settings.state_basedir is not None and
opts.aircraft is not None):
mpstate.aircraft_dir = os.path.join(mpstate.settings.state_basedir, opts.aircraft)
elif opts.aircraft is not None:
mpstate.aircraft_dir = opts.aircraft
run_startup_scripts()
if opts.cmd is not None:
for cstr in opts.cmd:
cmds = cstr.split(';')
for c in cmds:
process_stdin(c)
if opts.profile:
import yappi # We do the import here so that we won't barf if run normally and yappi not available
yappi.start()
# log all packets from the master, for later replay
if not opts.no_state:
open_telemetry_logs(logpath_telem, logpath_telem_raw)
else:
print("Note: Not saving telemetry logs")
# run main loop as a thread
mpstate.status.thread = threading.Thread(target=main_loop, name='main_loop')
mpstate.status.thread.daemon = True
mpstate.status.thread.start()
# use main program for input. This ensures the terminal cleans
# up on exit
while (mpstate.status.exit is not True):
try:
if opts.daemon or opts.non_interactive:
time.sleep(0.1)
else:
input_loop()
except KeyboardInterrupt:
if mpstate.settings.requireexit:
print("Interrupt caught. Use 'exit' to quit MAVProxy.")
# Just lost the map and console, get them back:
for (m, pm) in mpstate.modules:
if m.name in ["map", "console"]:
if hasattr(m, 'unload'):
try:
m.unload()
except Exception:
pass
reload(m)
m.init(mpstate)
else:
mpstate.status.exit = True
sys.exit(1)
if opts.profile:
yappi.get_func_stats().print_all()
yappi.get_thread_stats().print_all()
# this loop executes after leaving the above loop and is for cleanup on exit
for (m, pm) in mpstate.modules:
if hasattr(m, 'unload'):
print("Unloading module %s" % m.name)
m.unload()
sys.exit(1)
MAVProxy-1.8.74/MAVProxy/modules/ 0000775 0000000 0000000 00000000000 15043245150 0016434 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/MAVProxy/modules/__init__.py 0000664 0000000 0000000 00000000020 15043245150 0020535 0 ustar 00root root 0000000 0000000 # Allow imports
MAVProxy-1.8.74/MAVProxy/modules/lib/ 0000775 0000000 0000000 00000000000 15043245150 0017202 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/MAVProxy/modules/lib/ANUGA/ 0000775 0000000 0000000 00000000000 15043245150 0020035 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/MAVProxy/modules/lib/ANUGA/README.txt 0000664 0000000 0000000 00000000200 15043245150 0021523 0 ustar 00root root 0000000 0000000
This module adapted from ANUGA:
https://anuga.anu.edu.au/
Many thanks to ANUGA for making this code available under the GPL!
MAVProxy-1.8.74/MAVProxy/modules/lib/ANUGA/__init__.py 0000664 0000000 0000000 00000000000 15043245150 0022134 0 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/MAVProxy/modules/lib/ANUGA/geo_reference.py 0000664 0000000 0000000 00000040312 15043245150 0023177 0 ustar 00root root 0000000 0000000 """
This module adapted ANUGA
https://anuga.anu.edu.au/
"""
#FIXME: Ensure that all attributes of a georef are treated everywhere
#and unit test
import types, sys
import copy
import numpy as num
DEFAULT_ZONE = -1
TITLE = '#geo reference' + "\n" # this title is referred to in the test format
DEFAULT_PROJECTION = 'UTM'
DEFAULT_DATUM = 'wgs84'
DEFAULT_UNITS = 'm'
DEFAULT_FALSE_EASTING = 500000
DEFAULT_FALSE_NORTHING = 10000000 # Default for southern hemisphere
##
# @brief A class for ...
class Geo_reference:
"""
Attributes of the Geo_reference class:
.zone The UTM zone (default is -1)
.false_easting ??
.false_northing ??
.datum The Datum used (default is wgs84)
.projection The projection used (default is 'UTM')
.units The units of measure used (default metres)
.xllcorner The X coord of origin (default is 0.0 wrt UTM grid)
.yllcorner The y coord of origin (default is 0.0 wrt UTM grid)
.is_absolute ??
"""
##
# @brief Instantiate an instance of class Geo_reference.
# @param zone The UTM zone.
# @param xllcorner X coord of origin of georef.
# @param yllcorner Y coord of origin of georef.
# @param datum ??
# @param projection The projection used (default UTM).
# @param units Units used in measuring distance (default m).
# @param false_easting ??
# @param false_northing ??
# @param NetCDFObject NetCDF file *handle* to write to.
# @param ASCIIFile ASCII text file *handle* to write to.
# @param read_title Title of the georeference text.
def __init__(self,
zone=DEFAULT_ZONE,
xllcorner=0.0,
yllcorner=0.0,
datum=DEFAULT_DATUM,
projection=DEFAULT_PROJECTION,
units=DEFAULT_UNITS,
false_easting=DEFAULT_FALSE_EASTING,
false_northing=DEFAULT_FALSE_NORTHING,
NetCDFObject=None,
ASCIIFile=None,
read_title=None):
"""
input:
NetCDFObject - a handle to the netCDF file to be written to
ASCIIFile - a handle to the text file
read_title - the title of the georeference text, if it was read in.
If the function that calls this has already read the title line,
it can't unread it, so this info has to be passed.
If you know of a way to unread this info, then tell us.
Note, the text file only saves a sub set of the info the
points file does. Currently the info not written in text
must be the default info, since ANUGA assumes it isn't
changing.
"""
if zone is None:
zone = DEFAULT_ZONE
self.false_easting = int(false_easting)
self.false_northing = int(false_northing)
self.datum = datum
self.projection = projection
self.zone = int(zone)
self.units = units
self.xllcorner = float(xllcorner)
self.yllcorner = float(yllcorner)
if NetCDFObject is not None:
self.read_NetCDF(NetCDFObject)
if ASCIIFile is not None:
self.read_ASCII(ASCIIFile, read_title=read_title)
# Set flag for absolute points (used by get_absolute)
self.absolute = num.allclose([self.xllcorner, self.yllcorner], 0)
def get_xllcorner(self):
return self.xllcorner
##
# @brief Get the Y coordinate of the origin of this georef.
def get_yllcorner(self):
return self.yllcorner
##
# @brief Get the zone of this georef.
def get_zone(self):
return self.zone
##
# @brief Write to an open NetCDF file.
# @param outfile Handle to open NetCDF file.
def write_NetCDF(self, outfile):
outfile.xllcorner = self.xllcorner
outfile.yllcorner = self.yllcorner
outfile.zone = self.zone
outfile.false_easting = self.false_easting
outfile.false_northing = self.false_northing
outfile.datum = self.datum
outfile.projection = self.projection
outfile.units = self.units
##
# @brief Read data from an open NetCDF file.
# @param infile Handle to open NetCDF file.
def read_NetCDF(self, infile):
self.xllcorner = float(infile.xllcorner[0])
self.yllcorner = float(infile.yllcorner[0])
self.zone = int(infile.zone[0])
try:
self.false_easting = int(infile.false_easting[0])
self.false_northing = int(infile.false_northing[0])
self.datum = infile.datum
self.projection = infile.projection
self.units = infile.units
except:
pass
if self.false_easting != DEFAULT_FALSE_EASTING:
print("WARNING: False easting of %f specified." % self.false_easting)
print("Default false easting is %f." % DEFAULT_FALSE_EASTING)
print("ANUGA does not correct for differences in False Eastings.")
if self.false_northing != DEFAULT_FALSE_NORTHING:
print("WARNING: False northing of %f specified."
% self.false_northing)
print("Default false northing is %f." % DEFAULT_FALSE_NORTHING)
print("ANUGA does not correct for differences in False Northings.")
if self.datum.upper() != DEFAULT_DATUM.upper():
print("WARNING: Datum of %s specified." % self.datum)
print("Default Datum is %s." % DEFAULT_DATUM)
print("ANUGA does not correct for differences in datums.")
if self.projection.upper() != DEFAULT_PROJECTION.upper():
print("WARNING: Projection of %s specified." % self.projection)
print("Default Projection is %s." % DEFAULT_PROJECTION)
print("ANUGA does not correct for differences in Projection.")
if self.units.upper() != DEFAULT_UNITS.upper():
print("WARNING: Units of %s specified." % self.units)
print("Default units is %s." % DEFAULT_UNITS)
print("ANUGA does not correct for differences in units.")
################################################################################
# ASCII files with geo-refs are currently not used
################################################################################
##
# @brief Write georef data to an open text file.
# @param fd Handle to open text file.
def write_ASCII(self, fd):
fd.write(TITLE)
fd.write(str(self.zone) + "\n")
fd.write(str(self.xllcorner) + "\n")
fd.write(str(self.yllcorner) + "\n")
##
# @brief Read georef data from an open text file.
# @param fd Handle to open text file.
def read_ASCII(self, fd, read_title=None):
try:
if read_title is None:
read_title = fd.readline() # remove the title line
if read_title[0:2].upper() != TITLE[0:2].upper():
msg = ('File error. Expecting line: %s. Got this line: %s'
% (TITLE, read_title))
raise ValueError(msg)
self.zone = int(fd.readline())
self.xllcorner = float(fd.readline())
self.yllcorner = float(fd.readline())
except SyntaxError:
msg = 'File error. Got syntax error while parsing geo reference'
raise ValueError(msg)
# Fix some assertion failures
if isinstance(self.zone, num.ndarray) and self.zone.shape == ():
self.zone = self.zone[0]
if (isinstance(self.xllcorner, num.ndarray) and
self.xllcorner.shape == ()):
self.xllcorner = self.xllcorner[0]
if (isinstance(self.yllcorner, num.ndarray) and
self.yllcorner.shape == ()):
self.yllcorner = self.yllcorner[0]
assert (type(self.xllcorner) == types.FloatType)
assert (type(self.yllcorner) == types.FloatType)
assert (type(self.zone) == types.IntType)
################################################################################
##
# @brief Change points to be absolute wrt new georef 'points_geo_ref'.
# @param points The points to change.
# @param points_geo_ref The new georef to make points absolute wrt.
# @return The changed points.
# @note If 'points' is a list then a changed list is returned.
def change_points_geo_ref(self, points, points_geo_ref=None):
"""Change the geo reference of a list or numeric array of points to
be this reference.(The reference used for this object)
If the points do not have a geo ref, assume 'absolute' values
"""
import copy
# remember if we got a list
is_list = isinstance(points, list)
points = ensure_numeric(points, num.float)
# sanity checks
if len(points.shape) == 1:
#One point has been passed
msg = 'Single point must have two elements'
assert len(points) == 2, msg
points = num.reshape(points, (1,2))
msg = 'Points array must be two dimensional.\n'
msg += 'I got %d dimensions' %len(points.shape)
assert len(points.shape) == 2, msg
msg = 'Input must be an N x 2 array or list of (x,y) values. '
msg += 'I got an %d x %d array' %points.shape
assert points.shape[1] == 2, msg
# FIXME (Ole): Could also check if zone, xllcorner, yllcorner
# are identical in the two geo refs.
if points_geo_ref is not self:
# If georeferences are different
points = copy.copy(points) # Don't destroy input
if not points_geo_ref is None:
# Convert points to absolute coordinates
points[:,0] += points_geo_ref.xllcorner
points[:,1] += points_geo_ref.yllcorner
# Make points relative to primary geo reference
points[:,0] -= self.xllcorner
points[:,1] -= self.yllcorner
if is_list:
points = points.tolist()
return points
def is_absolute(self):
"""Return True if xllcorner==yllcorner==0 indicating that points
in question are absolute.
"""
# FIXME(Ole): It is unfortunate that decision about whether points
# are absolute or not lies with the georeference object. Ross pointed this out.
# Moreover, this little function is responsible for a large fraction of the time
# using in data fitting (something in like 40 - 50%.
# This was due to the repeated calls to allclose.
# With the flag method fitting is much faster (18 Mar 2009).
# FIXME(Ole): HACK to be able to reuse data already cached (18 Mar 2009).
# Remove at some point
if not hasattr(self, 'absolute'):
self.absolute = num.allclose([self.xllcorner, self.yllcorner], 0)
# Return absolute flag
return self.absolute
def get_absolute(self, points):
"""Given a set of points geo referenced to this instance,
return the points as absolute values.
"""
# remember if we got a list
is_list = isinstance(points, list)
points = ensure_numeric(points, num.float)
if len(points.shape) == 1:
# One point has been passed
msg = 'Single point must have two elements'
if not len(points) == 2:
raise ValueError(msg)
msg = 'Input must be an N x 2 array or list of (x,y) values. '
msg += 'I got an %d x %d array' %points.shape
if not points.shape[1] == 2:
raise ValueError(msg)
# Add geo ref to points
if not self.is_absolute():
points = copy.copy(points) # Don't destroy input
points[:,0] += self.xllcorner
points[:,1] += self.yllcorner
if is_list:
points = points.tolist()
return points
##
# @brief Convert points to relative measurement.
# @param points Points to convert to relative measurements.
# @return A set of points relative to the geo_reference instance.
def get_relative(self, points):
"""Given a set of points in absolute UTM coordinates,
make them relative to this geo_reference instance,
return the points as relative values.
This is the inverse of get_absolute.
"""
# remember if we got a list
is_list = isinstance(points, list)
points = ensure_numeric(points, num.float)
if len(points.shape) == 1:
#One point has been passed
msg = 'Single point must have two elements'
if not len(points) == 2:
raise ValueError(msg)
if not points.shape[1] == 2:
msg = ('Input must be an N x 2 array or list of (x,y) values. '
'I got an %d x %d array' % points.shape)
raise ValueError(msg)
# Subtract geo ref from points
if not self.is_absolute():
points = copy.copy(points) # Don't destroy input
points[:,0] -= self.xllcorner
points[:,1] -= self.yllcorner
if is_list:
points = points.tolist()
return points
##
# @brief ??
# @param other ??
def reconcile_zones(self, other):
if other is None:
other = Geo_reference()
if (self.zone == other.zone or
self.zone == DEFAULT_ZONE and
other.zone == DEFAULT_ZONE):
pass
elif self.zone == DEFAULT_ZONE:
self.zone = other.zone
elif other.zone == DEFAULT_ZONE:
other.zone = self.zone
else:
msg = ('Geospatial data must be in the same '
'ZONE to allow reconciliation. I got zone %d and %d'
% (self.zone, other.zone))
raise ValueError(msg)
#def easting_northing2geo_reffed_point(self, x, y):
# return [x-self.xllcorner, y - self.xllcorner]
#def easting_northing2geo_reffed_points(self, x, y):
# return [x-self.xllcorner, y - self.xllcorner]
##
# @brief Get origin of this geo_reference.
# @return (zone, xllcorner, yllcorner).
def get_origin(self):
return (self.zone, self.xllcorner, self.yllcorner)
##
# @brief Get a string representation of this geo_reference instance.
def __repr__(self):
return ('(zone=%i easting=%f, northing=%f)'
% (self.zone, self.xllcorner, self.yllcorner))
##
# @brief Compare two geo_reference instances.
# @param self This geo_reference instance.
# @param other Another geo_reference instance to compare against.
# @return 0 if instances have the same attributes, else 1.
# @note Attributes are: zone, xllcorner, yllcorner.
def __cmp__(self, other):
# FIXME (DSG) add a tolerance
if other is None:
return 1
cmp = 0
if not (self.xllcorner == self.xllcorner):
cmp = 1
if not (self.yllcorner == self.yllcorner):
cmp = 1
if not (self.zone == self.zone):
cmp = 1
return cmp
##
# @brief Write a geo_reference to a NetCDF file (usually SWW).
# @param origin A georef instance or parameters to create a georef instance.
# @param outfile Path to file to write.
# @return A normalized geo_reference.
def write_NetCDF_georeference(origin, outfile):
"""Write georeference info to a netcdf file, usually sww.
The origin can be a georef instance or parameters for a geo_ref instance
outfile is the name of the file to be written to.
"""
geo_ref = ensure_geo_reference(origin)
geo_ref.write_NetCDF(outfile)
return geo_ref
##
# @brief Convert an object to a georeference instance.
# @param origin A georef instance or (zone, xllcorner, yllcorner)
# @return A georef object, or None if 'origin' was None.
def ensure_geo_reference(origin):
"""
Given a list/tuple of zone, xllcorner and yllcorner of a geo-ref object,
return a geo ref object.
If the origin is None, return None, so calling this function doesn't
effect code logic
"""
if isinstance(origin, Geo_reference):
geo_ref = origin
elif origin is None:
geo_ref = None
else:
geo_ref = apply(Geo_reference, origin)
return geo_ref
#-----------------------------------------------------------------------
if __name__ == "__main__":
pass
MAVProxy-1.8.74/MAVProxy/modules/lib/ANUGA/lat_long_UTM_conversion.py 0000664 0000000 0000000 00000023320 15043245150 0025200 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
# Lat Long - UTM, UTM - Lat Long conversions
#
# see http://www.pygps.org
#
from math import pi, sin, cos, tan, sqrt
#LatLong- UTM conversion..h
#definitions for lat/long to UTM and UTM to lat/lng conversions
#include
_deg2rad = pi / 180.0
_rad2deg = 180.0 / pi
_EquatorialRadius = 2
_eccentricitySquared = 3
_ellipsoid = [
# id, Ellipsoid name, Equatorial Radius, square of eccentricity
# first once is a placeholder only, To allow array indices to match id numbers
[ -1, "Placeholder", 0, 0],
[ 1, "Airy", 6377563, 0.00667054],
[ 2, "Australian National", 6378160, 0.006694542],
[ 3, "Bessel 1841", 6377397, 0.006674372],
[ 4, "Bessel 1841 (Nambia] ", 6377484, 0.006674372],
[ 5, "Clarke 1866", 6378206, 0.006768658],
[ 6, "Clarke 1880", 6378249, 0.006803511],
[ 7, "Everest", 6377276, 0.006637847],
[ 8, "Fischer 1960 (Mercury] ", 6378166, 0.006693422],
[ 9, "Fischer 1968", 6378150, 0.006693422],
[ 10, "GRS 1967", 6378160, 0.006694605],
[ 11, "GRS 1980", 6378137, 0.00669438],
[ 12, "Helmert 1906", 6378200, 0.006693422],
[ 13, "Hough", 6378270, 0.00672267],
[ 14, "International", 6378388, 0.00672267],
[ 15, "Krassovsky", 6378245, 0.006693422],
[ 16, "Modified Airy", 6377340, 0.00667054],
[ 17, "Modified Everest", 6377304, 0.006637847],
[ 18, "Modified Fischer 1960", 6378155, 0.006693422],
[ 19, "South American 1969", 6378160, 0.006694542],
[ 20, "WGS 60", 6378165, 0.006693422],
[ 21, "WGS 66", 6378145, 0.006694542],
[ 22, "WGS-72", 6378135, 0.006694318],
[ 23, "WGS-84", 6378137, 0.00669438]
]
#Reference ellipsoids derived from Peter H. Dana's website-
#http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
#Department of Geography, University of Texas at Austin
#Internet: pdana@mail.utexas.edu
#3/22/95
#Source
#Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
#1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency
#def LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long,
# double &UTMNorthing, double &UTMEasting, char* UTMZone)
def LLtoUTM( Lat, Long, ReferenceEllipsoid=23):
"""
converts lat/long to UTM coords. Equations from USGS Bulletin 1532
East Longitudes are positive, West longitudes are negative.
North latitudes are positive, South latitudes are negative
Lat and Long are in decimal degrees
Written by Chuck Gantz- chuck.gantz@globalstar.com
"""
a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
k0 = 0.9996
#Make sure the longitude is between -180.00 .. 179.9
LongTemp = (Long+180)-int((Long+180)/360)*360-180 # -180.00 .. 179.9
LatRad = Lat*_deg2rad
LongRad = LongTemp*_deg2rad
ZoneNumber = int((LongTemp + 180)/6) + 1
if Lat >= 56.0 and Lat < 64.0 and LongTemp >= 3.0 and LongTemp < 12.0:
ZoneNumber = 32
# Special zones for Svalbard
if Lat >= 72.0 and Lat < 84.0:
if LongTemp >= 0.0 and LongTemp < 9.0:ZoneNumber = 31
elif LongTemp >= 9.0 and LongTemp < 21.0: ZoneNumber = 33
elif LongTemp >= 21.0 and LongTemp < 33.0: ZoneNumber = 35
elif LongTemp >= 33.0 and LongTemp < 42.0: ZoneNumber = 37
LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 #+3 puts origin in middle of zone
LongOriginRad = LongOrigin * _deg2rad
#compute the UTM Zone from the latitude and longitude
UTMZone = "%d%c" % (ZoneNumber, _UTMLetterDesignator(Lat))
eccPrimeSquared = (eccSquared)/(1-eccSquared)
N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad))
T = tan(LatRad)*tan(LatRad)
C = eccPrimeSquared*cos(LatRad)*cos(LatRad)
A = cos(LatRad)*(LongRad-LongOriginRad)
M = a*((1
- eccSquared/4
- 3*eccSquared*eccSquared/64
- 5*eccSquared*eccSquared*eccSquared/256)*LatRad
- (3*eccSquared/8
+ 3*eccSquared*eccSquared/32
+ 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
+ (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
- (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad))
UTMEasting = (k0*N*(A+(1-T+C)*A*A*A/6
+ (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
+ 500000.0)
UTMNorthing = (k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+ (61
-58*T
+T*T
+600*C
-330*eccPrimeSquared)*A*A*A*A*A*A/720)))
if Lat < 0:
UTMNorthing = UTMNorthing + 10000000.0; #10000000 meter offset for southern hemisphere
#UTMZone was originally returned here. I don't know what the
#letter at the end was for.
#print("UTMZone", UTMZone)
return (ZoneNumber, UTMEasting, UTMNorthing)
def _UTMLetterDesignator(Lat):
#This routine determines the correct UTM letter designator for the given latitude
#returns 'Z' if latitude is outside the UTM limits of 84N to 80S
#Written by Chuck Gantz- chuck.gantz@globalstar.com
if 84 >= Lat >= 72: return 'X'
elif 72 > Lat >= 64: return 'W'
elif 64 > Lat >= 56: return 'V'
elif 56 > Lat >= 48: return 'U'
elif 48 > Lat >= 40: return 'T'
elif 40 > Lat >= 32: return 'S'
elif 32 > Lat >= 24: return 'R'
elif 24 > Lat >= 16: return 'Q'
elif 16 > Lat >= 8: return 'P'
elif 8 > Lat >= 0: return 'N'
elif 0 > Lat >= -8: return 'M'
elif -8> Lat >= -16: return 'L'
elif -16 > Lat >= -24: return 'K'
elif -24 > Lat >= -32: return 'J'
elif -32 > Lat >= -40: return 'H'
elif -40 > Lat >= -48: return 'G'
elif -48 > Lat >= -56: return 'F'
elif -56 > Lat >= -64: return 'E'
elif -64 > Lat >= -72: return 'D'
elif -72 > Lat >= -80: return 'C'
else: return 'Z' # if the Latitude is outside the UTM limits
#void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
# double& Lat, double& Long )
def UTMtoLL(northing, easting, zone, isSouthernHemisphere=True,
ReferenceEllipsoid=23):
"""
converts UTM coords to lat/long. Equations from USGS Bulletin 1532
East Longitudes are positive, West longitudes are negative.
North latitudes are positive, South latitudes are negative
Lat and Long are in decimal degrees.
Written by Chuck Gantz- chuck.gantz@globalstar.com
Converted to Python by Russ Nelson
FIXME: This is set up to work for the Southern Hemisphere.
Using
http://www.ga.gov.au/geodesy/datums/redfearn_geo_to_grid.jsp
Site Name: GDA-MGA: (UTM with GRS80 ellipsoid)
Zone: 36
Easting: 511669.521 Northing: 19328195.112
Latitude: 84 0 ' 0.00000 '' Longitude: 34 0 ' 0.00000 ''
Grid Convergence: 0 -59 ' 40.28 '' Point Scale: 0.99960166
____________
Site Name: GDA-MGA: (UTM with GRS80 ellipsoid)
Zone: 36
Easting: 519384.803 Northing: 1118247.585
Latitude: -80 0 ' 0.00000 '' Longitude: 34 0 ' 0.00000 ''
Grid Convergence: 0 59 ' 5.32 '' Point Scale: 0.99960459
___________
Site Name: GDA-MGA: (UTM with GRS80 ellipsoid)
Zone: 36
Easting: 611263.812 Northing: 10110547.106
Latitude: 1 0 ' 0.00000 '' Longitude: 34 0 ' 0.00000 ''
Grid Convergence: 0 -1 ' 2.84 '' Point Scale: 0.99975325
______________
Site Name: GDA-MGA: (UTM with GRS80 ellipsoid)
Zone: 36
Easting: 611263.812 Northing: 9889452.894
Latitude: -1 0 ' 0.00000 '' Longitude: 34 0 ' 0.00000 ''
Grid Convergence: 0 1 ' 2.84 '' Point Scale: 0.99975325
So this uses a false northing of 10000000 in the both hemispheres.
ArcGIS used a false northing of 0 in the northern hem though.
Therefore it is difficult to actually know what hemisphere you are in.
"""
k0 = 0.9996
a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared))
x = easting - 500000.0 #remove 500,000 meter offset for longitude
y = northing
ZoneNumber = int(zone)
if isSouthernHemisphere:
y -= 10000000.0 # remove 10,000,000 meter offset used
# for southern hemisphere
LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 # +3 puts origin in middle of zone
eccPrimeSquared = (eccSquared)/(1-eccSquared)
M = y / k0
mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256))
phi1Rad = (mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
+ (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
+(151*e1*e1*e1/96)*sin(6*mu))
phi1 = phi1Rad*_rad2deg;
N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad))
T1 = tan(phi1Rad)*tan(phi1Rad)
C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad)
R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5)
D = x/(N1*k0)
Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720)
Lat = Lat * _rad2deg
Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
*D*D*D*D*D/120)/cos(phi1Rad)
Long = LongOrigin + Long * _rad2deg
return (Lat, Long)
if __name__ == '__main__':
(z, e, n) = LLtoUTM(45.00, -75.00, 23)
print(z, e, n)
(lat, lon) = UTMtoLL(n, e, z, 23)
print(lat, lon)
MAVProxy-1.8.74/MAVProxy/modules/lib/ANUGA/redfearn.py 0000664 0000000 0000000 00000014433 15043245150 0022202 0 ustar 00root root 0000000 0000000 """
This module adapted ANUGA
https://anuga.anu.edu.au/
------------
Implementation of Redfearn's formula to compute UTM projections from latitude and longitude
Based in part on spreadsheet
www.icsm.gov.au/gda/gdatm/redfearn.xls
downloaded from INTERGOVERNMENTAL COMMITTEE ON SURVEYING & MAPPING (ICSM)
http://www.icsm.gov.au/icsm/
"""
from MAVProxy.modules.lib.ANUGA.geo_reference import Geo_reference, DEFAULT_ZONE
def degminsec2decimal_degrees(dd,mm,ss):
assert abs(mm) == mm
assert abs(ss) == ss
if dd < 0:
sign = -1
else:
sign = 1
return sign * (abs(dd) + mm/60. + ss/3600.)
def decimal_degrees2degminsec(dec):
if dec < 0:
sign = -1
else:
sign = 1
dec = abs(dec)
dd = int(dec)
f = dec-dd
mm = int(f*60)
ss = (f*60-mm)*60
return sign*dd, mm, ss
def redfearn(lat, lon, false_easting=None, false_northing=None,
zone=None, central_meridian=None, scale_factor=None):
"""Compute UTM projection using Redfearn's formula
lat, lon is latitude and longitude in decimal degrees
If false easting and northing are specified they will override
the standard
If zone is specified reproject lat and long to specified zone instead of
standard zone
If meridian is specified, reproject lat and lon to that instead of zone. In this case
zone will be set to -1 to indicate non-UTM projection
Note that zone and meridian cannot both be specifed
"""
from math import pi, sqrt, sin, cos, tan
#GDA Specifications
a = 6378137.0 #Semi major axis
inverse_flattening = 298.257222101 #1/f
if scale_factor is None:
K0 = 0.9996 #Central scale factor
else:
K0 = scale_factor
#print('scale', K0)
zone_width = 6 #Degrees
longitude_of_central_meridian_zone0 = -183
longitude_of_western_edge_zone0 = -186
if false_easting is None:
false_easting = 500000
if false_northing is None:
if lat < 0:
false_northing = 10000000 #Southern hemisphere
else:
false_northing = 0 #Northern hemisphere)
#Derived constants
f = 1.0/inverse_flattening
b = a*(1-f) #Semi minor axis
e2 = 2*f - f*f# = f*(2-f) = (a^2-b^2/a^2 #Eccentricity
e = sqrt(e2)
e2_ = e2/(1-e2) # = (a^2-b^2)/b^2 #Second eccentricity
e_ = sqrt(e2_)
e4 = e2*e2
e6 = e2*e4
#Foot point latitude
n = (a-b)/(a+b) #Same as e2 - why ?
n2 = n*n
n3 = n*n2
n4 = n2*n2
G = a*(1-n)*(1-n2)*(1+9*n2/4+225*n4/64)*pi/180
phi = lat*pi/180 #Convert latitude to radians
sinphi = sin(phi)
sin2phi = sin(2*phi)
sin4phi = sin(4*phi)
sin6phi = sin(6*phi)
cosphi = cos(phi)
cosphi2 = cosphi*cosphi
cosphi3 = cosphi*cosphi2
cosphi4 = cosphi2*cosphi2
cosphi5 = cosphi*cosphi4
cosphi6 = cosphi2*cosphi4
cosphi7 = cosphi*cosphi6
cosphi8 = cosphi4*cosphi4
t = tan(phi)
t2 = t*t
t4 = t2*t2
t6 = t2*t4
#Radius of Curvature
rho = a*(1-e2)/(1-e2*sinphi*sinphi)**1.5
nu = a/(1-e2*sinphi*sinphi)**0.5
psi = nu/rho
psi2 = psi*psi
psi3 = psi*psi2
psi4 = psi2*psi2
#Meridian distance
A0 = 1 - e2/4 - 3*e4/64 - 5*e6/256
A2 = 3.0/8*(e2+e4/4+15*e6/128)
A4 = 15.0/256*(e4+3*e6/4)
A6 = 35*e6/3072
term1 = a*A0*phi
term2 = -a*A2*sin2phi
term3 = a*A4*sin4phi
term4 = -a*A6*sin6phi
m = term1 + term2 + term3 + term4 #OK
if zone is not None and central_meridian is not None:
msg = 'You specified both zone and central_meridian. Provide only one of them'
raise ValueError(msg)
# Zone
if zone is None:
zone = int((lon - longitude_of_western_edge_zone0)/zone_width)
# Central meridian
if central_meridian is None:
central_meridian = zone*zone_width+longitude_of_central_meridian_zone0
else:
zone = -1
omega = (lon-central_meridian)*pi/180 #Relative longitude (radians)
omega2 = omega*omega
omega3 = omega*omega2
omega4 = omega2*omega2
omega5 = omega*omega4
omega6 = omega3*omega3
omega7 = omega*omega6
omega8 = omega4*omega4
#Northing
term1 = nu*sinphi*cosphi*omega2/2
term2 = nu*sinphi*cosphi3*(4*psi2+psi-t2)*omega4/24
term3 = nu*sinphi*cosphi5*\
(8*psi4*(11-24*t2)-28*psi3*(1-6*t2)+\
psi2*(1-32*t2)-psi*2*t2+t4-t2)*omega6/720
term4 = nu*sinphi*cosphi7*(1385-3111*t2+543*t4-t6)*omega8/40320
northing = false_northing + K0*(m + term1 + term2 + term3 + term4)
#Easting
term1 = nu*omega*cosphi
term2 = nu*cosphi3*(psi-t2)*omega3/6
term3 = nu*cosphi5*(4*psi3*(1-6*t2)+psi2*(1+8*t2)-2*psi*t2+t4)*omega5/120
term4 = nu*cosphi7*(61-479*t2+179*t4-t6)*omega7/5040
easting = false_easting + K0*(term1 + term2 + term3 + term4)
return zone, easting, northing
def convert_from_latlon_to_utm(points=None,
latitudes=None,
longitudes=None,
false_easting=None,
false_northing=None):
"""Convert latitude and longitude data to UTM as a list of coordinates.
Input
points: list of points given in decimal degrees (latitude, longitude) or
latitudes: list of latitudes and
longitudes: list of longitudes
false_easting (optional)
false_northing (optional)
Output
points: List of converted points
zone: Common UTM zone for converted points
Notes
Assume the false_easting and false_northing are the same for each list.
If points end up in different UTM zones, an ANUGAerror is thrown.
"""
old_geo = Geo_reference()
utm_points = []
if points is None:
assert len(latitudes) == len(longitudes)
points = map(None, latitudes, longitudes)
for point in points:
zone, easting, northing = redfearn(float(point[0]),
float(point[1]),
false_easting=false_easting,
false_northing=false_northing)
new_geo = Geo_reference(zone)
old_geo.reconcile_zones(new_geo)
utm_points.append([easting, northing])
return utm_points, old_geo.get_zone()
MAVProxy-1.8.74/MAVProxy/modules/lib/LowPassFilter2p.py 0000664 0000000 0000000 00000003021 15043245150 0022550 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
from math import *
class LowPassFilter2p:
def __init__(self, sample_freq, cutoff_freq):
self.cutoff_freq = cutoff_freq
self.sample_freq = sample_freq
self.a1 = 0.0
self.a2 = 0.0
self.b0 = 0.0
self.b1 = 0.0
self.b2 = 0.0
self.delay_element_1 = None
self.delay_element_2 = None
self.set_cutoff_frequency(sample_freq, cutoff_freq)
def set_cutoff_frequency(self, sample_freq, cutoff_freq):
self.cutoff_freq = cutoff_freq
if self.cutoff_freq <= 0.0:
return
fr = self.sample_freq/self.cutoff_freq
ohm = tan(pi/fr)
c = 1.0+2.0*cos(pi/4.0)*ohm + ohm*ohm
self.b0 = ohm*ohm/c
self.b1 = 2.0*self.b0
self.b2 = self.b0
self.a1 = 2.0*(ohm*ohm-1.0)/c
self.a2 = (1.0-2.0*cos(pi/4.0)*ohm+ohm*ohm)/c
def apply(self, sample):
if self.delay_element_1 is None:
self.delay_element_1 = sample
self.delay_element_2 = sample
delay_element_0 = sample - (self.delay_element_1 * self.a1) - (self.delay_element_2 * self.a2)
output = (delay_element_0 * self.b0) + (self.delay_element_1 * self.b1) + (self.delay_element_2 * self.b2)
self.delay_element_2 = self.delay_element_1
self.delay_element_1 = delay_element_0
return output
if __name__ == "__main__":
from pymavlink.rotmat import Vector3
filter = LowPassFilter2p(1000.0, 98.0)
v2 = filter.apply(Vector3(1,2,3))
print(v2)
MAVProxy-1.8.74/MAVProxy/modules/lib/MacOS/ 0000775 0000000 0000000 00000000000 15043245150 0020144 5 ustar 00root root 0000000 0000000 MAVProxy-1.8.74/MAVProxy/modules/lib/MacOS/__init__.py 0000664 0000000 0000000 00000000076 15043245150 0022260 0 ustar 00root root 0000000 0000000 # version of matplotlib backends modified to work with MacOS
MAVProxy-1.8.74/MAVProxy/modules/lib/MacOS/backend_agg.py 0000664 0000000 0000000 00000045025 15043245150 0022731 0 ustar 00root root 0000000 0000000 """
An agg http://antigrain.com/ backend
Features that are implemented
* capstyles and join styles
* dashes
* linewidth
* lines, rectangles, ellipses
* clipping to a rectangle
* output to RGBA and PNG
* alpha blending
* DPI scaling properly - everything scales properly (dashes, linewidths, etc)
* draw polygon
* freetype2 w/ ft2font
TODO:
* integrate screen dpi w/ ppi and text
"""
from __future__ import division
import threading
import numpy as np
from matplotlib import verbose, rcParams
from matplotlib.backend_bases import RendererBase,\
FigureManagerBase, FigureCanvasBase
from matplotlib.cbook import is_string_like, maxdict
from matplotlib.figure import Figure
from matplotlib.font_manager import findfont
from matplotlib.ft2font import FT2Font, LOAD_FORCE_AUTOHINT, LOAD_NO_HINTING, \
LOAD_DEFAULT, LOAD_NO_AUTOHINT
from matplotlib.mathtext import MathTextParser
from matplotlib.path import Path
from matplotlib.transforms import Bbox, BboxBase
from matplotlib.backends._backend_agg import RendererAgg as _RendererAgg
from matplotlib import _png
backend_version = 'v2.2'
def get_hinting_flag():
mapping = {
True: LOAD_FORCE_AUTOHINT,
False: LOAD_NO_HINTING,
'either': LOAD_DEFAULT,
'native': LOAD_NO_AUTOHINT,
'auto': LOAD_FORCE_AUTOHINT,
'none': LOAD_NO_HINTING
}
return mapping[rcParams['text.hinting']]
class RendererAgg(RendererBase):
"""
The renderer handles all the drawing primitives using a graphics
context instance that controls the colors/styles
"""
debug=1
# we want to cache the fonts at the class level so that when
# multiple figures are created we can reuse them. This helps with
# a bug on windows where the creation of too many figures leads to
# too many open file handles. However, storing them at the class
# level is not thread safe. The solution here is to let the
# FigureCanvas acquire a lock on the fontd at the start of the
# draw, and release it when it is done. This allows multiple
# renderers to share the cached fonts, but only one figure can
# draw at at time and so the font cache is used by only one
# renderer at a time
lock = threading.RLock()
_fontd = maxdict(50)
def __init__(self, width, height, dpi):
if __debug__: verbose.report('RendererAgg.__init__', 'debug-annoying')
RendererBase.__init__(self)
self.texd = maxdict(50) # a cache of tex image rasters
self.dpi = dpi
self.width = width
self.height = height
if __debug__: verbose.report('RendererAgg.__init__ width=%s, height=%s'%(width, height), 'debug-annoying')
self._renderer = _RendererAgg(int(width), int(height), dpi, debug=False)
self._filter_renderers = []
if __debug__: verbose.report('RendererAgg.__init__ _RendererAgg done',
'debug-annoying')
self._update_methods()
self.mathtext_parser = MathTextParser('Agg')
self.bbox = Bbox.from_bounds(0, 0, self.width, self.height)
if __debug__: verbose.report('RendererAgg.__init__ done',
'debug-annoying')
def _get_hinting_flag(self):
if rcParams['text.hinting']:
return LOAD_FORCE_AUTOHINT
else:
return LOAD_NO_HINTING
# for filtering to work with rasterization, methods needs to be wrapped.
# maybe there is better way to do it.
def draw_markers(self, *kl, **kw):
return self._renderer.draw_markers(*kl, **kw)
def draw_path_collection(self, *kl, **kw):
return self._renderer.draw_path_collection(*kl, **kw)
def _update_methods(self):
#self.draw_path = self._renderer.draw_path # see below
#self.draw_markers = self._renderer.draw_markers
#self.draw_path_collection = self._renderer.draw_path_collection
self.draw_quad_mesh = self._renderer.draw_quad_mesh
self.draw_gouraud_triangle = self._renderer.draw_gouraud_triangle
self.draw_gouraud_triangles = self._renderer.draw_gouraud_triangles
self.draw_image = self._renderer.draw_image
self.copy_from_bbox = self._renderer.copy_from_bbox
def tostring_rgba_minimized(self):
extents = self._renderer.get_content_extents()
bbox = [[extents[0], self.height - (extents[1] + extents[3])],
[extents[0] + extents[2], self.height - extents[1]]]
region = self._renderer.copy_from_bbox(bbox)
return np.array(region), extents
def draw_path(self, gc, path, transform, rgbFace=None):
"""
Draw the path
"""
nmax = rcParams['agg.path.chunksize'] # here at least for testing
npts = path.vertices.shape[0]
if (nmax > 100 and npts > nmax and path.should_simplify and
rgbFace is None and gc.get_hatch() is None):
nch = np.ceil(npts/float(nmax))
chsize = int(np.ceil(npts/nch))
i0 = np.arange(0, npts, chsize)
i1 = np.zeros_like(i0)
i1[:-1] = i0[1:] - 1
i1[-1] = npts
for ii0, ii1 in zip(i0, i1):
v = path.vertices[ii0:ii1,:]
c = path.codes
if c is not None:
c = c[ii0:ii1]
c[0] = Path.MOVETO # move to end of last chunk
p = Path(v, c)
self._renderer.draw_path(gc, p, transform, rgbFace)
else:
self._renderer.draw_path(gc, path, transform, rgbFace)
def draw_mathtext(self, gc, x, y, s, prop, angle):
"""
Draw the math text using matplotlib.mathtext
"""
if __debug__: verbose.report('RendererAgg.draw_mathtext',
'debug-annoying')
ox, oy, width, height, descent, font_image, used_characters = \
self.mathtext_parser.parse(s, self.dpi, prop)
xd = descent * np.sin(np.deg2rad(angle))
yd = descent * np.cos(np.deg2rad(angle))
x = np.round(x + ox + xd)
y = np.round(y - oy + yd)
self._renderer.draw_text_image(font_image, x, y + 1, angle, gc)
def draw_text(self, gc, x, y, s, prop, angle, ismath=False, mtext=None):
"""
Render the text
"""
if __debug__: verbose.report('RendererAgg.draw_text', 'debug-annoying')
if ismath:
return self.draw_mathtext(gc, x, y, s, prop, angle)
flags = get_hinting_flag()
font = self._get_agg_font(prop)
if font is None: return None
if len(s) == 1 and ord(s) > 127:
font.load_char(ord(s), flags=flags)
else:
# We pass '0' for angle here, since it will be rotated (in raster
# space) in the following call to draw_text_image).
font.set_text(s, 0, flags=flags)
font.draw_glyphs_to_bitmap(antialiased=rcParams['text.antialiased'])
d = font.get_descent() / 64.0
# The descent needs to be adjusted for the angle
xd = -d * np.sin(np.deg2rad(angle))
yd = d * np.cos(np.deg2rad(angle))
#print x, y, int(x), int(y), s
self._renderer.draw_text_image(
font.get_image(), np.round(x - xd), np.round(y + yd) + 1, angle, gc)
def get_text_width_height_descent(self, s, prop, ismath):
"""
get the width and height in display coords of the string s
with FontPropertry prop
# passing rgb is a little hack to make cacheing in the
# texmanager more efficient. It is not meant to be used
# outside the backend
"""
if rcParams['text.usetex']:
# todo: handle props
size = prop.get_size_in_points()
texmanager = self.get_texmanager()
fontsize = prop.get_size_in_points()
w, h, d = texmanager.get_text_width_height_descent(s, fontsize,
renderer=self)
return w, h, d
if ismath:
ox, oy, width, height, descent, fonts, used_characters = \
self.mathtext_parser.parse(s, self.dpi, prop)
return width, height, descent
flags = get_hinting_flag()
font = self._get_agg_font(prop)
font.set_text(s, 0.0, flags=flags) # the width and height of unrotated string
w, h = font.get_width_height()
d = font.get_descent()
w /= 64.0 # convert from subpixels
h /= 64.0
d /= 64.0
return w, h, d
def draw_tex(self, gc, x, y, s, prop, angle, ismath='TeX!', mtext=None):
# todo, handle props, angle, origins
size = prop.get_size_in_points()
texmanager = self.get_texmanager()
key = s, size, self.dpi, angle, texmanager.get_font_config()
im = self.texd.get(key)
if im is None:
Z = texmanager.get_grey(s, size, self.dpi)
Z = np.array(Z * 255.0, np.uint8)
w, h, d = self.get_text_width_height_descent(s, prop, ismath)
xd = d * np.sin(np.deg2rad(angle))
yd = d * np.cos(np.deg2rad(angle))
x = np.round(x + xd)
y = np.round(y + yd)
self._renderer.draw_text_image(Z, x, y, angle, gc)
def get_canvas_width_height(self):
'return the canvas width and height in display coords'
return self.width, self.height
def _get_agg_font(self, prop):
"""
Get the font for text instance t, cacheing for efficiency
"""
if __debug__: verbose.report('RendererAgg._get_agg_font',
'debug-annoying')
key = hash(prop)
font = RendererAgg._fontd.get(key)
if font is None:
fname = findfont(prop)
font = RendererAgg._fontd.get(fname)
if font is None:
font = FT2Font(
str(fname),
hinting_factor=rcParams['text.hinting_factor'])
RendererAgg._fontd[fname] = font
RendererAgg._fontd[key] = font
font.clear()
size = prop.get_size_in_points()
font.set_size(size, self.dpi)
return font
def points_to_pixels(self, points):
"""
convert point measures to pixes using dpi and the pixels per
inch of the display
"""
if __debug__: verbose.report('RendererAgg.points_to_pixels',
'debug-annoying')
return points*self.dpi/72.0
def tostring_rgb(self):
if __debug__: verbose.report('RendererAgg.tostring_rgb',
'debug-annoying')
return self._renderer.tostring_rgb()
def tostring_argb(self):
if __debug__: verbose.report('RendererAgg.tostring_argb',
'debug-annoying')
return self._renderer.tostring_argb()
def buffer_rgba(self):
if __debug__: verbose.report('RendererAgg.buffer_rgba',
'debug-annoying')
return self._renderer.buffer_rgba()
def clear(self):
self._renderer.clear()
def option_image_nocomposite(self):
# It is generally faster to composite each image directly to
# the Figure, and there's no file size benefit to compositing
# with the Agg backend
return True
def option_scale_image(self):
"""
agg backend support arbitrary scaling of image.
"""
return True
def restore_region(self, region, bbox=None, xy=None):
"""
Restore the saved region. If bbox (instance of BboxBase, or
its extents) is given, only the region specified by the bbox
will be restored. *xy* (a tuple of two floasts) optionally
specifies the new position (the LLC of the original region,
not the LLC of the bbox) where the region will be restored.
>>> region = renderer.copy_from_bbox()
>>> x1, y1, x2, y2 = region.get_extents()
>>> renderer.restore_region(region, bbox=(x1+dx, y1, x2, y2),
... xy=(x1-dx, y1))
"""
if bbox is not None or xy is not None:
if bbox is None:
x1, y1, x2, y2 = region.get_extents()
elif isinstance(bbox, BboxBase):
x1, y1, x2, y2 = bbox.extents
else:
x1, y1, x2, y2 = bbox
if xy is None:
ox, oy = x1, y1
else:
ox, oy = xy
self._renderer.restore_region2(region, x1, y1, x2, y2, ox, oy)
else:
self._renderer.restore_region(region)
def start_filter(self):
"""
Start filtering. It simply create a new canvas (the old one is saved).
"""
self._filter_renderers.append(self._renderer)
self._renderer = _RendererAgg(int(self.width), int(self.height),
self.dpi)
self._update_methods()
def stop_filter(self, post_processing):
"""
Save the plot in the current canvas as a image and apply
the *post_processing* function.
def post_processing(image, dpi):
# ny, nx, depth = image.shape
# image (numpy array) has RGBA channels and has a depth of 4.
...
# create a new_image (numpy array of 4 channels, size can be
# different). The resulting image may have offsets from
# lower-left corner of the original image
return new_image, offset_x, offset_y
The saved renderer is restored and the returned image from
post_processing is plotted (using draw_image) on it.
"""
# WARNING.
# For agg_filter to work, the rendere's method need
# to overridden in the class. See draw_markers, and draw_path_collections
from matplotlib._image import fromarray
width, height = int(self.width), int(self.height)
buffer, bounds = self._renderer.tostring_rgba_minimized()
l, b, w, h = bounds
self._renderer = self._filter_renderers.pop()
self._update_methods()
if w > 0 and h > 0:
img = np.fromstring(buffer, np.uint8)
img, ox, oy = post_processing(img.reshape((h, w, 4)) / 255.,
self.dpi)
image = fromarray(img, 1)
image.flipud_out()
gc = self.new_gc()
self._renderer.draw_image(gc,
l+ox, height - b - h +oy,
image)
def new_figure_manager(num, *args, **kwargs):
"""
Create a new figure manager instance
"""
if __debug__: verbose.report('backend_agg.new_figure_manager',
'debug-annoying')
FigureClass = kwargs.pop('FigureClass', Figure)
thisFig = FigureClass(*args, **kwargs)
return new_figure_manager_given_figure(num, thisFig)
def new_figure_manager_given_figure(num, figure):
"""
Create a new figure manager instance for the given figure.
"""
canvas = FigureCanvasAgg(figure)
manager = FigureManagerBase(canvas, num)
return manager
class FigureCanvasAgg(FigureCanvasBase):
"""
The canvas the figure renders into. Calls the draw and print fig
methods, creates the renderers, etc...
Public attribute
figure - A Figure instance
"""
def copy_from_bbox(self, bbox):
renderer = self.get_renderer()
return renderer.copy_from_bbox(bbox)
def restore_region(self, region, bbox=None, xy=None):
renderer = self.get_renderer()
return renderer.restore_region(region, bbox, xy)
def draw(self):
"""
Draw the figure using the renderer
"""
if __debug__: verbose.report('FigureCanvasAgg.draw', 'debug-annoying')
self.renderer = self.get_renderer(cleared=True)
# acquire a lock on the shared font cache
RendererAgg.lock.acquire()
try:
self.figure.draw(self.renderer)
finally:
RendererAgg.lock.release()
def get_renderer(self, cleared=False):
l, b, w, h = self.figure.bbox.bounds
key = w, h, self.figure.dpi
try: self._lastKey, self.renderer
except AttributeError: need_new_renderer = True
else: need_new_renderer = (self._lastKey != key)
if need_new_renderer:
self.renderer = RendererAgg(w, h, self.figure.dpi)
self._lastKey = key
elif cleared:
self.renderer.clear()
return self.renderer
def tostring_rgb(self):
if __debug__: verbose.report('FigureCanvasAgg.tostring_rgb',
'debug-annoying')
return self.renderer.tostring_rgb()
def tostring_argb(self):
if __debug__: verbose.report('FigureCanvasAgg.tostring_argb',
'debug-annoying')
return self.renderer.tostring_argb()
def buffer_rgba(self):
if __debug__: verbose.report('FigureCanvasAgg.buffer_rgba',
'debug-annoying')
return self.renderer.buffer_rgba()
def print_raw(self, filename_or_obj, *args, **kwargs):
FigureCanvasAgg.draw(self)
renderer = self.get_renderer()
original_dpi = renderer.dpi
renderer.dpi = self.figure.dpi
if is_string_like(filename_or_obj):
filename_or_obj = open(filename_or_obj, 'wb')
close = True
else:
close = False
try:
renderer._renderer.write_rgba(filename_or_obj)
finally:
if close:
filename_or_obj.close()
renderer.dpi = original_dpi
print_rgba = print_raw
def print_png(self, filename_or_obj, *args, **kwargs):
FigureCanvasAgg.draw(self)
renderer = self.get_renderer()
original_dpi = renderer.dpi
renderer.dpi = self.figure.dpi
if is_string_like(filename_or_obj):
filename_or_obj = open(filename_or_obj, 'wb')
close = True
else:
close = False
try:
_png.write_png(renderer._renderer.buffer_rgba(),
renderer.width, renderer.height,
filename_or_obj, self.figure.dpi)
finally:
if close:
filename_or_obj.close()
renderer.dpi = original_dpi
def print_to_buffer(self):
FigureCanvasAgg.draw(self)
renderer = self.get_renderer()
original_dpi = renderer.dpi
renderer.dpi = self.figure.dpi
result = (renderer._renderer.buffer_rgba(),
(int(renderer.width), int(renderer.height)))
renderer.dpi = original_dpi
return result
MAVProxy-1.8.74/MAVProxy/modules/lib/MacOS/backend_wx.py 0000664 0000000 0000000 00000215741 15043245150 0022635 0 ustar 00root root 0000000 0000000 from __future__ import division, print_function
"""
A wxPython backend for matplotlib, based (very heavily) on
backend_template.py and backend_gtk.py
Author: Jeremy O'Donoghue (jeremy@o-donoghue.com)
Derived from original copyright work by John Hunter
(jdhunter@ace.bsd.uchicago.edu)
Copyright (C) Jeremy O'Donoghue & John Hunter, 2003-4
License: This work is licensed under a PSF compatible license. A copy
should be included with this source code.
"""
import sys
import os
import os.path
import math
import StringIO
import weakref
import warnings
import numpy as np
from matplotlib.cbook import mplDeprecation
# Debugging settings here...
# Debug level set here. If the debug level is less than 5, information
# messages (progressively more info for lower value) are printed. In addition,
# traceback is performed, and pdb activated, for all uncaught exceptions in
# this case
_DEBUG = 5
if _DEBUG < 5:
import traceback, pdb
_DEBUG_lvls = {1 : 'Low ', 2 : 'Med ', 3 : 'High', 4 : 'Error' }
if sys.version_info[0] >= 3:
warnings.warn(
"The wx and wxagg backends have not been tested with Python 3.x",
ImportWarning)
missingwx = "Matplotlib backend_wx and backend_wxagg require wxPython >=2.8"
missingwxversion = ("Matplotlib backend_wx and backend_wxagg "
"require wxversion, which was not found.")
if not hasattr(sys, 'frozen'): # i.e., not py2exe
try:
import wxversion
except ImportError:
raise ImportError(missingwxversion)
# Some early versions of wxversion lack AlreadyImportedError.
# It was added around 2.8.4?
try:
_wx_ensure_failed = wxversion.AlreadyImportedError
except AttributeError:
_wx_ensure_failed = wxversion.VersionError
try:
wxversion.ensureMinimal('2.8')
except _wx_ensure_failed:
pass
# We don't really want to pass in case of VersionError, but when
# AlreadyImportedError is not available, we have to.
try:
import wx
backend_version = wx.VERSION_STRING
except ImportError:
raise ImportError(missingwx)
# Extra version check in case wxversion lacks AlreadyImportedError;
# then VersionError might have been raised and ignored when
# there really *is* a problem with the version.
major, minor = [int(n) for n in backend_version.split('.')[:2]]
if major < 2 or (major < 3 and minor < 8):
print(" wxPython version %s was imported." % backend_version)
raise ImportError(missingwx)
#!!! this is the call that is causing the exception swallowing !!!
#wx.InitAllImageHandlers()
def DEBUG_MSG(string, lvl=3, o=None):
if lvl >= _DEBUG:
cls = o.__class__
# Jeremy, often times the commented line won't print but the
# one below does. I think WX is redefining stderr, damned
# beast
#print >>sys.stderr, "%s- %s in %s" % (_DEBUG_lvls[lvl], string, cls)
print("%s- %s in %s" % (_DEBUG_lvls[lvl], string, cls))
def debug_on_error(type, value, tb):
"""Code due to Thomas Heller - published in Python Cookbook (O'Reilley)"""
traceback.print_exc(type, value, tb)
print()
pdb.pm() # jdh uncomment
class fake_stderr:
"""Wx does strange things with stderr, as it makes the assumption that there
is probably no console. This redirects stderr to the console, since we know
that there is one!"""
def write(self, msg):
print("Stderr: %s\n\r" % msg)
#if _DEBUG < 5:
# sys.excepthook = debug_on_error
# WxLogger =wx.LogStderr()
# sys.stderr = fake_stderr
# Event binding code changed after version 2.5
if wx.VERSION_STRING >= '2.5':
def bind(actor,event,action,**kw):
actor.Bind(event,action,**kw)
else:
def bind(actor,event,action,id=None):
if id is not None:
event(actor, id, action)
else:
event(actor,action)
import matplotlib
from matplotlib import verbose
from matplotlib.backend_bases import RendererBase, GraphicsContextBase,\
FigureCanvasBase, FigureManagerBase, NavigationToolbar2, \
cursors, TimerBase
from matplotlib.backend_bases import ShowBase
from matplotlib.backend_bases import _has_pil
from matplotlib._pylab_helpers import Gcf
from matplotlib.artist import Artist
from matplotlib.cbook import exception_to_str, is_string_like, is_writable_file_like
from matplotlib.figure import Figure
from matplotlib.path import Path
from matplotlib.text import _process_text_args, Text
from matplotlib.transforms import Affine2D
from matplotlib.widgets import SubplotTool
from matplotlib import rcParams
# the True dots per inch on the screen; should be display dependent
# see http://groups.google.com/groups?q=screen+dpi+x11&hl=en&lr=&ie=UTF-8&oe=UTF-8&safe=off&selm=7077.26e81ad5%40swift.cs.tcd.ie&rnum=5 for some info about screen dpi
PIXELS_PER_INCH = 75
# Delay time for idle checks
IDLE_DELAY = 5
def error_msg_wx(msg, parent=None):
"""
Signal an error condition -- in a GUI, popup a error dialog
"""
dialog =wx.MessageDialog(parent = parent,
message = msg,
caption = 'Matplotlib backend_wx error',
style=wx.OK | wx.CENTRE)
dialog.ShowModal()
dialog.Destroy()
return None
def raise_msg_to_str(msg):
"""msg is a return arg from a raise. Join with new lines"""
if not is_string_like(msg):
msg = '\n'.join(map(str, msg))
return msg
class TimerWx(TimerBase):
'''
Subclass of :class:`backend_bases.TimerBase` that uses WxTimer events.
Attributes:
* interval: The time between timer events in milliseconds. Default
is 1000 ms.
* single_shot: Boolean flag indicating whether this timer should
operate as single shot (run once and then stop). Defaults to False.
* callbacks: Stores list of (func, args) tuples that will be called
upon timer events. This list can be manipulated directly, or the
functions add_callback and remove_callback can be used.
'''
def __init__(self, parent, *args, **kwargs):
TimerBase.__init__(self, *args, **kwargs)
# Create a new timer and connect the timer event to our handler.
# For WX, the events have to use a widget for binding.
self.parent = parent
self._timer = wx.Timer(self.parent, wx.NewId())
self.parent.Bind(wx.EVT_TIMER, self._on_timer, self._timer)
# Unbinding causes Wx to stop for some reason. Disabling for now.
# def __del__(self):
# TimerBase.__del__(self)
# self.parent.Bind(wx.EVT_TIMER, None, self._timer)
def _timer_start(self):
self._timer.Start(self._interval, self._single)
def _timer_stop(self):
self._timer.Stop()
def _timer_set_interval(self):
self._timer_start()
def _timer_set_single_shot(self):
self._timer.start()
def _on_timer(self, *args):
TimerBase._on_timer(self)
class RendererWx(RendererBase):
"""
The renderer handles all the drawing primitives using a graphics
context instance that controls the colors/styles. It acts as the
'renderer' instance used by many classes in the hierarchy.
"""
#In wxPython, drawing is performed on a wxDC instance, which will
#generally be mapped to the client aread of the window displaying
#the plot. Under wxPython, the wxDC instance has a wx.Pen which
#describes the colour and weight of any lines drawn, and a wxBrush
#which describes the fill colour of any closed polygon.
fontweights = {
100 : wx.LIGHT,
200 : wx.LIGHT,
300 : wx.LIGHT,
400 : wx.NORMAL,
500 : wx.NORMAL,
600 : wx.NORMAL,
700 : wx.BOLD,
800 : wx.BOLD,
900 : wx.BOLD,
'ultralight' : wx.LIGHT,
'light' : wx.LIGHT,
'normal' : wx.NORMAL,
'medium' : wx.NORMAL,
'semibold' : wx.NORMAL,
'bold' : wx.BOLD,
'heavy' : wx.BOLD,
'ultrabold' : wx.BOLD,
'black' : wx.BOLD
}
fontangles = {
'italic' : wx.ITALIC,
'normal' : wx.NORMAL,
'oblique' : wx.SLANT }
# wxPython allows for portable font styles, choosing them appropriately
# for the target platform. Map some standard font names to the portable
# styles
# QUESTION: Is it be wise to agree standard fontnames across all backends?
fontnames = { 'Sans' : wx.SWISS,
'Roman' : wx.ROMAN,
'Script' : wx.SCRIPT,
'Decorative' : wx.DECORATIVE,
'Modern' : wx.MODERN,
'Courier' : wx.MODERN,
'courier' : wx.MODERN }
def __init__(self, bitmap, dpi):
"""
Initialise a wxWindows renderer instance.
"""
RendererBase.__init__(self)
DEBUG_MSG("__init__()", 1, self)
if wx.VERSION_STRING < "2.8":
raise RuntimeError("matplotlib no longer supports wxPython < 2.8 for the Wx backend.\nYou may, however, use the WxAgg backend.")
self.width = bitmap.GetWidth()
self.height = bitmap.GetHeight()
self.bitmap = bitmap
self.fontd = {}
self.dpi = dpi
self.gc = None
def flipy(self):
return True
def offset_text_height(self):
return True
def get_text_width_height_descent(self, s, prop, ismath):
"""
get the width and height in display coords of the string s
with FontPropertry prop
"""
#return 1, 1
if ismath: s = self.strip_math(s)
if self.gc is None:
gc = self.new_gc()
else:
gc = self.gc
gfx_ctx = gc.gfx_ctx
font = self.get_wx_font(s, prop)
gfx_ctx.SetFont(font, wx.BLACK)
w, h, descent, leading = gfx_ctx.GetFullTextExtent(s)
return w, h, descent
def get_canvas_width_height(self):
'return the canvas width and height in display coords'
return self.width, self.height
def handle_clip_rectangle(self, gc):
new_bounds = gc.get_clip_rectangle()
if new_bounds is not None:
new_bounds = new_bounds.bounds
gfx_ctx = gc.gfx_ctx
if gfx_ctx._lastcliprect != new_bounds:
gfx_ctx._lastcliprect = new_bounds
if new_bounds is None:
gfx_ctx.ResetClip()
else:
gfx_ctx.Clip(new_bounds[0], self.height - new_bounds[1] - new_bounds[3],
new_bounds[2], new_bounds[3])
@staticmethod
def convert_path(gfx_ctx, path, transform):
wxpath = gfx_ctx.CreatePath()
for points, code in path.iter_segments(transform):
if code == Path.MOVETO:
wxpath.MoveToPoint(*points)
elif code == Path.LINETO:
wxpath.AddLineToPoint(*points)
elif code == Path.CURVE3:
wxpath.AddQuadCurveToPoint(*points)
elif code == Path.CURVE4:
wxpath.AddCurveToPoint(*points)
elif code == Path.CLOSEPOLY:
wxpath.CloseSubpath()
return wxpath
def draw_path(self, gc, path, transform, rgbFace=None):
gc.select()
self.handle_clip_rectangle(gc)
gfx_ctx = gc.gfx_ctx
transform = transform + Affine2D().scale(1.0, -1.0).translate(0.0, self.height)
wxpath = self.convert_path(gfx_ctx, path, transform)
if rgbFace is not None:
gfx_ctx.SetBrush(wx.Brush(gc.get_wxcolour(rgbFace)))
gfx_ctx.DrawPath(wxpath)
else:
gfx_ctx.StrokePath(wxpath)
gc.unselect()
def draw_image(self, gc, x, y, im):
bbox = gc.get_clip_rectangle()
if bbox != None:
l,b,w,h = bbox.bounds
else:
l=0
b=0,
w=self.width
h=self.height
rows, cols, image_str = im.as_rgba_str()
image_array = np.fromstring(image_str, np.uint8)
image_array.shape = rows, cols, 4
bitmap = wx.BitmapFromBufferRGBA(cols,rows,image_array)
gc = self.get_gc()
gc.select()
gc.gfx_ctx.DrawBitmap(bitmap,int(l),int(self.height-b),int(w),int(-h))
gc.unselect()
def draw_text(self, gc, x, y, s, prop, angle, ismath=False, mtext=None):
if ismath: s = self.strip_math(s)
DEBUG_MSG("draw_text()", 1, self)
gc.select()
self.handle_clip_rectangle(gc)
gfx_ctx = gc.gfx_ctx
font = self.get_wx_font(s, prop)
color = gc.get_wxcolour(gc.get_rgb())
gfx_ctx.SetFont(font, color)
w, h, d = self.get_text_width_height_descent(s, prop, ismath)
x = int(x)
y = int(y-h)
if angle == 0.0:
gfx_ctx.DrawText(s, x, y)
else:
rads = angle / 180.0 * math.pi
xo = h * math.sin(rads)
yo = h * math.cos(rads)
gfx_ctx.DrawRotatedText(s, x - xo, y - yo, rads)
gc.unselect()
def new_gc(self):
"""
Return an instance of a GraphicsContextWx, and sets the current gc copy
"""
DEBUG_MSG('new_gc()', 2, self)
self.gc = GraphicsContextWx(self.bitmap, self)
self.gc.select()
self.gc.unselect()
return self.gc
def get_gc(self):
"""
Fetch the locally cached gc.
"""
# This is a dirty hack to allow anything with access to a renderer to
# access the current graphics context
assert self.gc != None, "gc must be defined"
return self.gc
def get_wx_font(self, s, prop):
"""
Return a wx font. Cache instances in a font dictionary for
efficiency
"""
DEBUG_MSG("get_wx_font()", 1, self)
key = hash(prop)
fontprop = prop
fontname = fontprop.get_name()
font = self.fontd.get(key)
if font is not None:
return font
# Allow use of platform independent and dependent font names
wxFontname = self.fontnames.get(fontname, wx.ROMAN)
wxFacename = '' # Empty => wxPython chooses based on wx_fontname
# Font colour is determined by the active wx.Pen
# TODO: It may be wise to cache font information
size = self.points_to_pixels(fontprop.get_size_in_points())
font =wx.Font(int(size+0.5), # Size
wxFontname, # 'Generic' name
self.fontangles[fontprop.get_style()], # Angle
self.fontweights[fontprop.get_weight()], # Weight
False, # Underline
wxFacename) # Platform font name
# cache the font and gc and return it
self.fontd[key] = font
return font
def points_to_pixels(self, points):
"""
convert point measures to pixes using dpi and the pixels per
inch of the display
"""
return points*(PIXELS_PER_INCH/72.0*self.dpi/72.0)
class GraphicsContextWx(GraphicsContextBase):
"""
The graphics context provides the color, line styles, etc...
This class stores a reference to a wxMemoryDC, and a
wxGraphicsContext that draws to it. Creating a wxGraphicsContext
seems to be fairly heavy, so these objects are cached based on the
bitmap object that is passed in.
The base GraphicsContext stores colors as a RGB tuple on the unit
interval, eg, (0.5, 0.0, 1.0). wxPython uses an int interval, but
since wxPython colour management is rather simple, I have not chosen
to implement a separate colour manager class.
"""
_capd = { 'butt': wx.CAP_BUTT,
'projecting': wx.CAP_PROJECTING,
'round': wx.CAP_ROUND }
_joind = { 'bevel': wx.JOIN_BEVEL,
'miter': wx.JOIN_MITER,
'round': wx.JOIN_ROUND }
_dashd_wx = { 'solid': wx.SOLID,
'dashed': wx.SHORT_DASH,
'dashdot': wx.DOT_DASH,
'dotted': wx.DOT }
_cache = weakref.WeakKeyDictionary()
def __init__(self, bitmap, renderer):
GraphicsContextBase.__init__(self)
#assert self.Ok(), "wxMemoryDC not OK to use"
DEBUG_MSG("__init__()", 1, self)
dc, gfx_ctx = self._cache.get(bitmap, (None, None))
if dc is None:
dc = wx.MemoryDC()
dc.SelectObject(bitmap)
gfx_ctx = wx.GraphicsContext.Create(dc)
gfx_ctx._lastcliprect = None
self._cache[bitmap] = dc, gfx_ctx
self.bitmap = bitmap
self.dc = dc
self.gfx_ctx = gfx_ctx
self._pen = wx.Pen('BLACK', 1, wx.SOLID)
gfx_ctx.SetPen(self._pen)
self._style = wx.SOLID
self.renderer = renderer
def select(self):
"""
Select the current bitmap into this wxDC instance
"""
if sys.platform=='win32':
self.dc.SelectObject(self.bitmap)
self.IsSelected = True
def unselect(self):
"""
Select a Null bitmasp into this wxDC instance
"""
if sys.platform=='win32':
self.dc.SelectObject(wx.NullBitmap)
self.IsSelected = False
def set_foreground(self, fg, isRGBA=None):
"""
Set the foreground color. fg can be a matlab format string, a
html hex color string, an rgb unit tuple, or a float between 0
and 1. In the latter case, grayscale is used.
"""
# Implementation note: wxPython has a separate concept of pen and
# brush - the brush fills any outline trace left by the pen.
# Here we set both to the same colour - if a figure is not to be
# filled, the renderer will set the brush to be transparent
# Same goes for text foreground...
DEBUG_MSG("set_foreground()", 1, self)
self.select()
GraphicsContextBase.set_foreground(self, fg, isRGBA)
self._pen.SetColour(self.get_wxcolour(self.get_rgb()))
self.gfx_ctx.SetPen(self._pen)
self.unselect()
def set_graylevel(self, frac):
"""
Set the foreground color. fg can be a matlab format string, a
html hex color string, an rgb unit tuple, or a float between 0
and 1. In the latter case, grayscale is used.
"""
DEBUG_MSG("set_graylevel()", 1, self)
self.select()
GraphicsContextBase.set_graylevel(self, frac)
self._pen.SetColour(self.get_wxcolour(self.get_rgb()))
self.gfx_ctx.SetPen(self._pen)
self.unselect()
def set_linewidth(self, w):
"""
Set the line width.
"""
DEBUG_MSG("set_linewidth()", 1, self)
self.select()
if w>0 and w<1: w = 1
GraphicsContextBase.set_linewidth(self, w)
lw = int(self.renderer.points_to_pixels(self._linewidth))
if lw==0: lw = 1
self._pen.SetWidth(lw)
self.gfx_ctx.SetPen(self._pen)
self.unselect()
def set_capstyle(self, cs):
"""
Set the capstyle as a string in ('butt', 'round', 'projecting')
"""
DEBUG_MSG("set_capstyle()", 1, self)
self.select()
GraphicsContextBase.set_capstyle(self, cs)
self._pen.SetCap(GraphicsContextWx._capd[self._capstyle])
self.gfx_ctx.SetPen(self._pen)
self.unselect()
def set_joinstyle(self, js):
"""
Set the join style to be one of ('miter', 'round', 'bevel')
"""
DEBUG_MSG("set_joinstyle()", 1, self)
self.select()
GraphicsContextBase.set_joinstyle(self, js)
self._pen.SetJoin(GraphicsContextWx._joind[self._joinstyle])
self.gfx_ctx.SetPen(self._pen)
self.unselect()
def set_linestyle(self, ls):
"""
Set the line style to be one of
"""
DEBUG_MSG("set_linestyle()", 1, self)
self.select()
GraphicsContextBase.set_linestyle(self, ls)
try:
self._style = GraphicsContextWx._dashd_wx[ls]
except KeyError:
self._style = wx.LONG_DASH# Style not used elsewhere...
# On MS Windows platform, only line width of 1 allowed for dash lines
if wx.Platform == '__WXMSW__':
self.set_linewidth(1)
self._pen.SetStyle(self._style)
self.gfx_ctx.SetPen(self._pen)
self.unselect()
def get_wxcolour(self, color):
"""return a wx.Colour from RGB format"""
DEBUG_MSG("get_wx_color()", 1, self)
if len(color) == 3:
r, g, b = color
r *= 255
g *= 255
b *= 255
return wx.Colour(red=int(r), green=int(g), blue=int(b))
else:
r, g, b, a = color
r *= 255
g *= 255
b *= 255
a *= 255
return wx.Colour(red=int(r), green=int(g), blue=int(b), alpha=int(a))
class FigureCanvasWx(FigureCanvasBase, wx.Panel):
"""
The FigureCanvas contains the figure and does event handling.
In the wxPython backend, it is derived from wxPanel, and (usually) lives
inside a frame instantiated by a FigureManagerWx. The parent window probably
implements a wx.Sizer to control the displayed control size - but we give a
hint as to our preferred minimum size.
"""
keyvald = {
wx.WXK_CONTROL : 'control',
wx.WXK_SHIFT : 'shift',
wx.WXK_ALT : 'alt',
wx.WXK_LEFT : 'left',
wx.WXK_UP : 'up',
wx.WXK_RIGHT : 'right',
wx.WXK_DOWN : 'down',
wx.WXK_ESCAPE : 'escape',
wx.WXK_F1 : 'f1',
wx.WXK_F2 : 'f2',
wx.WXK_F3 : 'f3',
wx.WXK_F4 : 'f4',
wx.WXK_F5 : 'f5',
wx.WXK_F6 : 'f6',
wx.WXK_F7 : 'f7',
wx.WXK_F8 : 'f8',
wx.WXK_F9 : 'f9',
wx.WXK_F10 : 'f10',
wx.WXK_F11 : 'f11',
wx.WXK_F12 : 'f12',
wx.WXK_SCROLL : 'scroll_lock',
wx.WXK_PAUSE : 'break',
wx.WXK_BACK : 'backspace',
wx.WXK_RETURN : 'enter',
wx.WXK_INSERT : 'insert',
wx.WXK_DELETE : 'delete',
wx.WXK_HOME : 'home',
wx.WXK_END : 'end',
#wx.WXK_PRIOR : 'pageup',
#wx.WXK_NEXT : 'pagedown',
wx.WXK_PAGEUP : 'pageup',
wx.WXK_PAGEDOWN : 'pagedown',
wx.WXK_NUMPAD0 : '0',
wx.WXK_NUMPAD1 : '1',
wx.WXK_NUMPAD2 : '2',
wx.WXK_NUMPAD3 : '3',
wx.WXK_NUMPAD4 : '4',
wx.WXK_NUMPAD5 : '5',
wx.WXK_NUMPAD6 : '6',
wx.WXK_NUMPAD7 : '7',
wx.WXK_NUMPAD8 : '8',
wx.WXK_NUMPAD9 : '9',
wx.WXK_NUMPAD_ADD : '+',
wx.WXK_NUMPAD_SUBTRACT : '-',
wx.WXK_NUMPAD_MULTIPLY : '*',
wx.WXK_NUMPAD_DIVIDE : '/',
wx.WXK_NUMPAD_DECIMAL : 'dec',
wx.WXK_NUMPAD_ENTER : 'enter',
wx.WXK_NUMPAD_UP : 'up',
wx.WXK_NUMPAD_RIGHT : 'right',
wx.WXK_NUMPAD_DOWN : 'down',
wx.WXK_NUMPAD_LEFT : 'left',
#wx.WXK_NUMPAD_PRIOR : 'pageup',
#wx.WXK_NUMPAD_NEXT : 'pagedown',
wx.WXK_NUMPAD_PAGEUP : 'pageup',
wx.WXK_NUMPAD_PAGEDOWN : 'pagedown',
wx.WXK_NUMPAD_HOME : 'home',
wx.WXK_NUMPAD_END : 'end',
wx.WXK_NUMPAD_INSERT : 'insert',
wx.WXK_NUMPAD_DELETE : 'delete',
}
def __init__(self, parent, id, figure):
"""
Initialise a FigureWx instance.
- Initialise the FigureCanvasBase and wxPanel parents.
- Set event handlers for:
EVT_SIZE (Resize event)
EVT_PAINT (Paint event)
"""
FigureCanvasBase.__init__(self, figure)
# Set preferred window size hint - helps the sizer (if one is
# connected)
l,b,w,h = figure.bbox.bounds
w = int(math.ceil(w))
h = int(math.ceil(h))
wx.Panel.__init__(self, parent, id, size=wx.Size(w, h))
def do_nothing(*args, **kwargs):
warnings.warn('could not find a setinitialsize function for backend_wx; please report your wxpython version=%s to the matplotlib developers list'%backend_version)
pass
# try to find the set size func across wx versions
try:
getattr(self, 'SetInitialSize')
except AttributeError:
self.SetInitialSize = getattr(self, 'SetBestFittingSize', do_nothing)
if not hasattr(self,'IsShownOnScreen'):
self.IsShownOnScreen = getattr(self, 'IsVisible', lambda *args: True)
# Create the drawing bitmap
self.bitmap =wx.EmptyBitmap(w, h)
DEBUG_MSG("__init__() - bitmap w:%d h:%d" % (w,h), 2, self)
# TODO: Add support for 'point' inspection and plot navigation.
self._isDrawn = False
bind(self, wx.EVT_SIZE, self._onSize)
bind(self, wx.EVT_PAINT, self._onPaint)
bind(self, wx.EVT_ERASE_BACKGROUND, self._onEraseBackground)
bind(self, wx.EVT_KEY_DOWN, self._onKeyDown)
bind(self, wx.EVT_KEY_UP, self._onKeyUp)
bind(self, wx.EVT_RIGHT_DOWN, self._onRightButtonDown)
bind(self, wx.EVT_RIGHT_DCLICK, self._onRightButtonDClick)
bind(self, wx.EVT_RIGHT_UP, self._onRightButtonUp)
bind(self, wx.EVT_MOUSEWHEEL, self._onMouseWheel)
bind(self, wx.EVT_LEFT_DOWN, self._onLeftButtonDown)
bind(self, wx.EVT_LEFT_DCLICK, self._onLeftButtonDClick)
bind(self, wx.EVT_LEFT_UP, self._onLeftButtonUp)
bind(self, wx.EVT_MOTION, self._onMotion)
bind(self, wx.EVT_LEAVE_WINDOW, self._onLeave)
bind(self, wx.EVT_ENTER_WINDOW, self._onEnter)
bind(self, wx.EVT_IDLE, self._onIdle)
#Add middle button events
bind(self, wx.EVT_MIDDLE_DOWN, self._onMiddleButtonDown)
bind(self, wx.EVT_MIDDLE_DCLICK, self._onMiddleButtonDClick)
bind(self, wx.EVT_MIDDLE_UP, self._onMiddleButtonUp)
self.SetBackgroundStyle(wx.BG_STYLE_CUSTOM)
self.macros = {} # dict from wx id to seq of macros
# printer attributes and methods deprecated, 2010/06/19
self._printerData = None
self._printerPageData = None
self.printer_width = 5.5
self.printer_margin = 0.5
def Destroy(self, *args, **kwargs):
wx.Panel.Destroy(self, *args, **kwargs)
def Copy_to_Clipboard(self, event=None):
"copy bitmap of canvas to system clipboard"
bmp_obj = wx.BitmapDataObject()
bmp_obj.SetBitmap(self.bitmap)
if not wx.TheClipboard.IsOpened():
open_success = wx.TheClipboard.Open()
if open_success:
wx.TheClipboard.SetData(bmp_obj)
wx.TheClipboard.Close()
wx.TheClipboard.Flush()
def draw_idle(self):
"""
Delay rendering until the GUI is idle.
"""
DEBUG_MSG("draw_idle()", 1, self)
self._isDrawn = False # Force redraw
# Create a timer for handling draw_idle requests
# If there are events pending when the timer is
# complete, reset the timer and continue. The
# alternative approach, binding to wx.EVT_IDLE,
# doesn't behave as nicely.
if hasattr(self,'_idletimer'):
self._idletimer.Restart(IDLE_DELAY)
else:
self._idletimer = wx.FutureCall(IDLE_DELAY,self._onDrawIdle)
# FutureCall is a backwards-compatible alias;
# CallLater became available in 2.7.1.1.
def _onDrawIdle(self, *args, **kwargs):
if wx.GetApp().Pending():
self._idletimer.Restart(IDLE_DELAY, *args, **kwargs)
else:
del self._idletimer
# GUI event or explicit draw call may already
# have caused the draw to take place
if not self._isDrawn:
self.draw(*args, **kwargs)
def draw(self, drawDC=None):
"""
Render the figure using RendererWx instance renderer, or using a
previously defined renderer if none is specified.
"""
DEBUG_MSG("draw()", 1, self)
self.renderer = RendererWx(self.bitmap, self.figure.dpi)
self.figure.draw(self.renderer)
self._isDrawn = True
self.gui_repaint(drawDC=drawDC)
def new_timer(self, *args, **kwargs):
"""
Creates a new backend-specific subclass of :class:`backend_bases.Timer`.
This is useful for getting periodic events through the backend's native
event loop. Implemented only for backends with GUIs.
optional arguments:
*interval*
Timer interval in milliseconds
*callbacks*
Sequence of (func, args, kwargs) where func(*args, **kwargs) will
be executed by the timer every *interval*.
"""
return TimerWx(self, *args, **kwargs)
def flush_events(self):
wx.Yield()
def start_event_loop(self, timeout=0):
"""
Start an event loop. This is used to start a blocking event
loop so that interactive functions, such as ginput and
waitforbuttonpress, can wait for events. This should not be
confused with the main GUI event loop, which is always running
and has nothing to do with this.
Call signature::
start_event_loop(self,timeout=0)
This call blocks until a callback function triggers
stop_event_loop() or *timeout* is reached. If *timeout* is
<=0, never timeout.
Raises RuntimeError if event loop is already running.
"""
if hasattr(self, '_event_loop'):
raise RuntimeError("Event loop already running")
id = wx.NewId()
timer = wx.Timer(self, id=id)
if timeout > 0:
timer.Start(timeout*1000, oneShot=True)
bind(self, wx.EVT_TIMER, self.stop_event_loop, id=id)
# Event loop handler for start/stop event loop
self._event_loop = wx.EventLoop()
self._event_loop.Run()
timer.Stop()
def stop_event_loop(self, event=None):
"""
Stop an event loop. This is used to stop a blocking event
loop so that interactive functions, such as ginput and
waitforbuttonpress, can wait for events.
Call signature::
stop_event_loop_default(self)
"""
if hasattr(self,'_event_loop'):
if self._event_loop.IsRunning():
self._event_loop.Exit()
del self._event_loop
def _get_imagesave_wildcards(self):
'return the wildcard string for the filesave dialog'
default_filetype = self.get_default_filetype()
filetypes = self.get_supported_filetypes_grouped()
sorted_filetypes = filetypes.items()
sorted_filetypes.sort()
wildcards = []
extensions = []
filter_index = 0
for i, (name, exts) in enumerate(sorted_filetypes):
ext_list = ';'.join(['*.%s' % ext for ext in exts])
extensions.append(exts[0])
wildcard = '%s (%s)|%s' % (name, ext_list, ext_list)
if default_filetype in exts:
filter_index = i
wildcards.append(wildcard)
wildcards = '|'.join(wildcards)
return wildcards, extensions, filter_index
def gui_repaint(self, drawDC=None):
"""
Performs update of the displayed image on the GUI canvas, using the
supplied device context. If drawDC is None, a ClientDC will be used to
redraw the image.
"""
DEBUG_MSG("gui_repaint()", 1, self)
if self.IsShownOnScreen():
if drawDC is None:
drawDC=wx.ClientDC(self)
#drawDC.BeginDrawing()
drawDC.DrawBitmap(self.bitmap, 0, 0)
#drawDC.EndDrawing()
#wx.GetApp().Yield()
else:
pass
filetypes = FigureCanvasBase.filetypes.copy()
filetypes['bmp'] = 'Windows bitmap'
filetypes['jpeg'] = 'JPEG'
filetypes['jpg'] = 'JPEG'
filetypes['pcx'] = 'PCX'
filetypes['png'] = 'Portable Network Graphics'
filetypes['tif'] = 'Tagged Image Format File'
filetypes['tiff'] = 'Tagged Image Format File'
filetypes['xpm'] = 'X pixmap'
def print_figure(self, filename, *args, **kwargs):
# Use pure Agg renderer to draw
FigureCanvasBase.print_figure(self, filename, *args, **kwargs)
# Restore the current view; this is needed because the
# artist contains methods rely on particular attributes
# of the rendered figure for determining things like
# bounding boxes.
if self._isDrawn:
self.draw()
def print_bmp(self, filename, *args, **kwargs):
return self._print_image(filename, wx.BITMAP_TYPE_BMP, *args, **kwargs)
if not _has_pil:
def print_jpeg(self, filename, *args, **kwargs):
return self._print_image(filename, wx.BITMAP_TYPE_JPEG, *args, **kwargs)
print_jpg = print_jpeg
def print_pcx(self, filename, *args, **kwargs):
return self._print_image(filename, wx.BITMAP_TYPE_PCX, *args, **kwargs)
def print_png(self, filename, *args, **kwargs):
return self._print_image(filename, wx.BITMAP_TYPE_PNG, *args, **kwargs)
if not _has_pil:
def print_tiff(self, filename, *args, **kwargs):
return self._print_image(filename, wx.BITMAP_TYPE_TIF, *args, **kwargs)
print_tif = print_tiff
def print_xpm(self, filename, *args, **kwargs):
return self._print_image(filename, wx.BITMAP_TYPE_XPM, *args, **kwargs)
def _print_image(self, filename, filetype, *args, **kwargs):
origBitmap = self.bitmap
l,b,width,height = self.figure.bbox.bounds
width = int(math.ceil(width))
height = int(math.ceil(height))
self.bitmap = wx.EmptyBitmap(width, height)
renderer = RendererWx(self.bitmap, self.figure.dpi)
gc = renderer.new_gc()
self.figure.draw(renderer)
# image is the object that we call SaveFile on.
image = self.bitmap
# set the JPEG quality appropriately. Unfortunately, it is only possible
# to set the quality on a wx.Image object. So if we are saving a JPEG,
# convert the wx.Bitmap to a wx.Image, and set the quality.
if filetype == wx.BITMAP_TYPE_JPEG:
jpeg_quality = kwargs.get('quality',rcParams['savefig.jpeg_quality'])
image = self.bitmap.ConvertToImage()
image.SetOption(wx.IMAGE_OPTION_QUALITY,str(jpeg_quality))
# Now that we have rendered into the bitmap, save it
# to the appropriate file type and clean up
if is_string_like(filename):
if not image.SaveFile(filename, filetype):
DEBUG_MSG('print_figure() file save error', 4, self)
raise RuntimeError('Could not save figure to %s\n' % (filename))
elif is_writable_file_like(filename):
if not isinstance(image,wx.Image):
image = image.ConvertToImage()
if not image.SaveStream(filename, filetype):
DEBUG_MSG('print_figure() file save error', 4, self)
raise RuntimeError('Could not save figure to %s\n' % (filename))
# Restore everything to normal
self.bitmap = origBitmap
# Note: draw is required here since bits of state about the
# last renderer are strewn about the artist draw methods. Do
# not remove the draw without first verifying that these have
# been cleaned up. The artist contains() methods will fail
# otherwise.
if self._isDrawn:
self.draw()
self.Refresh()
def _onPaint(self, evt):
"""
Called when wxPaintEvt is generated
"""
DEBUG_MSG("_onPaint()", 1, self)
drawDC = wx.PaintDC(self)
if not self._isDrawn:
self.draw(drawDC=drawDC)
else:
self.gui_repaint(drawDC=drawDC)
evt.Skip()
def _onEraseBackground(self, evt):
"""
Called when window is redrawn; since we are blitting the entire
image, we can leave this blank to suppress flicker.
"""
pass
def _onSize(self, evt):
"""
Called when wxEventSize is generated.
In this application we attempt to resize to fit the window, so it
is better to take the performance hit and redraw the whole window.
"""
DEBUG_MSG("_onSize()", 2, self)
# Create a new, correctly sized bitmap
self._width, self._height = self.GetClientSize()
self.bitmap =wx.EmptyBitmap(self._width, self._height)
self._isDrawn = False
if self._width <= 1 or self._height <= 1: return # Empty figure
dpival = self.figure.dpi
winch = self._width/dpival
hinch = self._height/dpival
self.figure.set_size_inches(winch, hinch)
# Rendering will happen on the associated paint event
# so no need to do anything here except to make sure
# the whole background is repainted.
self.Refresh(eraseBackground=False)
FigureCanvasBase.resize_event(self)
def _get_key(self, evt):
keyval = evt.m_keyCode
if keyval in self.keyvald:
key = self.keyvald[keyval]
elif keyval < 256:
key = chr(keyval)
# wx always returns an uppercase, so make it lowercase if the shift
# key is not depressed (NOTE: this will not handle Caps Lock)
if not evt.ShiftDown():
key = key.lower()
else:
key = None
for meth, prefix in (
[evt.AltDown, 'alt'],
[evt.ControlDown, 'ctrl'], ):
if meth():
key = '{0}+{1}'.format(prefix, key)
return key
def _onIdle(self, evt):
'a GUI idle event'
evt.Skip()
FigureCanvasBase.idle_event(self, guiEvent=evt)
def _onKeyDown(self, evt):
"""Capture key press."""
key = self._get_key(evt)
evt.Skip()
FigureCanvasBase.key_press_event(self, key, guiEvent=evt)
def _onKeyUp(self, evt):
"""Release key."""
key = self._get_key(evt)
#print 'release key', key
evt.Skip()
FigureCanvasBase.key_release_event(self, key, guiEvent=evt)
def _onRightButtonDown(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
self.CaptureMouse()
FigureCanvasBase.button_press_event(self, x, y, 3, guiEvent=evt)
def _onRightButtonDClick(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
self.CaptureMouse()
FigureCanvasBase.button_press_event(self, x, y, 3, dblclick=True,guiEvent=evt)
def _onRightButtonUp(self, evt):
"""End measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
if self.HasCapture(): self.ReleaseMouse()
FigureCanvasBase.button_release_event(self, x, y, 3, guiEvent=evt)
def _onLeftButtonDown(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
self.CaptureMouse()
FigureCanvasBase.button_press_event(self, x, y, 1, guiEvent=evt)
def _onLeftButtonDClick(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
self.CaptureMouse()
FigureCanvasBase.button_press_event(self, x, y, 1, dblclick=True, guiEvent=evt)
def _onLeftButtonUp(self, evt):
"""End measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
#print 'release button', 1
evt.Skip()
if self.HasCapture(): self.ReleaseMouse()
FigureCanvasBase.button_release_event(self, x, y, 1, guiEvent=evt)
#Add middle button events
def _onMiddleButtonDown(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
self.CaptureMouse()
FigureCanvasBase.button_press_event(self, x, y, 2, guiEvent=evt)
def _onMiddleButtonDClick(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
self.CaptureMouse()
FigureCanvasBase.button_press_event(self, x, y, 2, dblclick=True, guiEvent=evt)
def _onMiddleButtonUp(self, evt):
"""End measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
#print 'release button', 1
evt.Skip()
if self.HasCapture(): self.ReleaseMouse()
FigureCanvasBase.button_release_event(self, x, y, 2, guiEvent=evt)
def _onMouseWheel(self, evt):
"""Translate mouse wheel events into matplotlib events"""
# Determine mouse location
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
# Convert delta/rotation/rate into a floating point step size
delta = evt.GetWheelDelta()
rotation = evt.GetWheelRotation()
rate = evt.GetLinesPerAction()
#print "delta,rotation,rate",delta,rotation,rate
step = rate*float(rotation)/delta
# Done handling event
evt.Skip()
# Mac is giving two events for every wheel event
# Need to skip every second one
if wx.Platform == '__WXMAC__':
if not hasattr(self,'_skipwheelevent'):
self._skipwheelevent = True
elif self._skipwheelevent:
self._skipwheelevent = False
return # Return without processing event
else:
self._skipwheelevent = True
# Convert to mpl event
FigureCanvasBase.scroll_event(self, x, y, step, guiEvent=evt)
def _onMotion(self, evt):
"""Start measuring on an axis."""
x = evt.GetX()
y = self.figure.bbox.height - evt.GetY()
evt.Skip()
FigureCanvasBase.motion_notify_event(self, x, y, guiEvent=evt)
def _onLeave(self, evt):
"""Mouse has left the window."""
evt.Skip()
FigureCanvasBase.leave_notify_event(self, guiEvent = evt)
def _onEnter(self, evt):
"""Mouse has entered the window."""
FigureCanvasBase.enter_notify_event(self, guiEvent = evt)
########################################################################
#
# The following functions and classes are for pylab compatibility
# mode (matplotlib.pylab) and implement figure managers, etc...
#
########################################################################
def _create_wx_app():
"""
Creates a wx.App instance if it has not been created sofar.
"""
wxapp = wx.GetApp()
if wxapp is None:
wxapp = wx.App(False)
wxapp.SetExitOnFrameDelete(True)
# retain a reference to the app object so it does not get garbage
# collected and cause segmentation faults
_create_wx_app.theWxApp = wxapp
def draw_if_interactive():
"""
This should be overriden in a windowing environment if drawing
should be done in interactive python mode
"""
DEBUG_MSG("draw_if_interactive()", 1, None)
if matplotlib.is_interactive():
figManager = Gcf.get_active()
if figManager is not None:
figManager.canvas.draw_idle()
class Show(ShowBase):
def mainloop(self):
needmain = not wx.App.IsMainLoopRunning()
if needmain:
wxapp = wx.GetApp()
if wxapp is not None:
wxapp.MainLoop()
show = Show()
def new_figure_manager(num, *args, **kwargs):
"""
Create a new figure manager instance
"""
# in order to expose the Figure constructor to the pylab
# interface we need to create the figure here
DEBUG_MSG("new_figure_manager()", 3, None)
_create_wx_app()
FigureClass = kwargs.pop('FigureClass', Figure)
fig = FigureClass(*args, **kwargs)
return new_figure_manager_given_figure(num, fig)
def new_figure_manager_given_figure(num, figure):
"""
Create a new figure manager instance for the given figure.
"""
fig = figure
frame = FigureFrameWx(num, fig)
figmgr = frame.get_figure_manager()
if matplotlib.is_interactive():
figmgr.frame.Show()
return figmgr
class FigureFrameWx(wx.Frame):
def __init__(self, num, fig):
# On non-Windows platform, explicitly set the position - fix
# positioning bug on some Linux platforms
if wx.Platform == '__WXMSW__':
pos = wx.DefaultPosition
else:
pos =wx.Point(20,20)
l,b,w,h = fig.bbox.bounds
wx.Frame.__init__(self, parent=None, id=-1, pos=pos,
title="Figure %d" % num)
# Frame will be sized later by the Fit method
DEBUG_MSG("__init__()", 1, self)
self.num = num
statbar = StatusBarWx(self)
self.SetStatusBar(statbar)
self.canvas = self.get_canvas(fig)
self.canvas.SetInitialSize(wx.Size(fig.bbox.width, fig.bbox.height))
self.canvas.SetFocus()
self.sizer =wx.BoxSizer(wx.VERTICAL)
self.sizer.Add(self.canvas, 1, wx.TOP | wx.LEFT | wx.EXPAND)
# By adding toolbar in sizer, we are able to put it at the bottom
# of the frame - so appearance is closer to GTK version
self.toolbar = self._get_toolbar(statbar)
if self.toolbar is not None:
self.toolbar.Realize()
# On Windows platform, default window size is incorrect, so set
# toolbar width to figure width.
tw, th = self.toolbar.GetSizeTuple()
fw, fh = self.canvas.GetSizeTuple()
# By adding toolbar in sizer, we are able to put it at the bottom
# of the frame - so appearance is closer to GTK version.
self.toolbar.SetSize(wx.Size(fw, th))
self.sizer.Add(self.toolbar, 0, wx.LEFT | wx.EXPAND)
self.SetSizer(self.sizer)
self.Fit()
self.canvas.SetMinSize((2, 2))
# give the window a matplotlib icon rather than the stock one.
# This is not currently working on Linux and is untested elsewhere.
#icon_path = os.path.join(matplotlib.rcParams['datapath'],
# 'images', 'matplotlib.png')
#icon = wx.IconFromBitmap(wx.Bitmap(icon_path))
# for xpm type icons try:
#icon = wx.Icon(icon_path, wx.BITMAP_TYPE_XPM)
#self.SetIcon(icon)
self.figmgr = FigureManagerWx(self.canvas, num, self)
bind(self, wx.EVT_CLOSE, self._onClose)
def _get_toolbar(self, statbar):
if rcParams['toolbar']=='classic':
toolbar = NavigationToolbarWx(self.canvas, True)
elif rcParams['toolbar']=='toolbar2':
toolbar = NavigationToolbar2Wx(self.canvas)
toolbar.set_status_bar(statbar)
else:
toolbar = None
return toolbar
def get_canvas(self, fig):
return FigureCanvasWx(self, -1, fig)
def get_figure_manager(self):
DEBUG_MSG("get_figure_manager()", 1, self)
return self.figmgr
def _onClose(self, evt):
DEBUG_MSG("onClose()", 1, self)
self.canvas.close_event()
self.canvas.stop_event_loop()
Gcf.destroy(self.num)
#self.Destroy()
def GetToolBar(self):
"""Override wxFrame::GetToolBar as we don't have managed toolbar"""
return self.toolbar
def Destroy(self, *args, **kwargs):
try:
self.canvas.mpl_disconnect(self.toolbar._idDrag)
# Rationale for line above: see issue 2941338.
except AttributeError:
pass # classic toolbar lacks the attribute
wx.Frame.Destroy(self, *args, **kwargs)
if self.toolbar is not None:
self.toolbar.Destroy()
wxapp = wx.GetApp()
if wxapp:
wxapp.Yield()
return True
class FigureManagerWx(FigureManagerBase):
"""
This class contains the FigureCanvas and GUI frame
It is instantiated by GcfWx whenever a new figure is created. GcfWx is
responsible for managing multiple instances of FigureManagerWx.
public attrs
canvas - a FigureCanvasWx(wx.Panel) instance
window - a wxFrame instance - http://www.lpthe.jussieu.fr/~zeitlin/wxWindows/docs/wxwin_wxframe.html#wxframe
"""
def __init__(self, canvas, num, frame):
DEBUG_MSG("__init__()", 1, self)
FigureManagerBase.__init__(self, canvas, num)
self.frame = frame
self.window = frame
self.tb = frame.GetToolBar()
self.toolbar = self.tb # consistent with other backends
def notify_axes_change(fig):
'this will be called whenever the current axes is changed'
if self.tb != None: self.tb.update()
self.canvas.figure.add_axobserver(notify_axes_change)
def show(self):
self.frame.Show()
def destroy(self, *args):
DEBUG_MSG("destroy()", 1, self)
self.frame.Destroy()
#if self.tb is not None: self.tb.Destroy()
#wx.GetApp().ProcessIdle()
wx.WakeUpIdle()
def get_window_title(self):
return self.window.GetTitle()
def set_window_title(self, title):
self.window.SetTitle(title)
def resize(self, width, height):
'Set the canvas size in pixels'
self.canvas.SetInitialSize(wx.Size(width, height))
self.window.GetSizer().Fit(self.window)
# Identifiers for toolbar controls - images_wx contains bitmaps for the images
# used in the controls. wxWindows does not provide any stock images, so I've
# 'stolen' those from GTK2, and transformed them into the appropriate format.
#import images_wx
_NTB_AXISMENU =wx.NewId()
_NTB_AXISMENU_BUTTON =wx.NewId()
_NTB_X_PAN_LEFT =wx.NewId()
_NTB_X_PAN_RIGHT =wx.NewId()
_NTB_X_ZOOMIN =wx.NewId()
_NTB_X_ZOOMOUT =wx.NewId()
_NTB_Y_PAN_UP =wx.NewId()
_NTB_Y_PAN_DOWN =wx.NewId()
_NTB_Y_ZOOMIN =wx.NewId()
_NTB_Y_ZOOMOUT =wx.NewId()
#_NTB_SUBPLOT =wx.NewId()
_NTB_SAVE =wx.NewId()
_NTB_CLOSE =wx.NewId()
def _load_bitmap(filename):
"""
Load a bitmap file from the backends/images subdirectory in which the
matplotlib library is installed. The filename parameter should not
contain any path information as this is determined automatically.
Returns a wx.Bitmap object
"""
basedir = os.path.join(rcParams['datapath'],'images')
bmpFilename = os.path.normpath(os.path.join(basedir, filename))
if not os.path.exists(bmpFilename):
raise IOError('Could not find bitmap file "%s"; dying'%bmpFilename)
bmp = wx.Bitmap(bmpFilename)
return bmp
class MenuButtonWx(wx.Button):
"""
wxPython does not permit a menu to be incorporated directly into a toolbar.
This class simulates the effect by associating a pop-up menu with a button
in the toolbar, and managing this as though it were a menu.
"""
def __init__(self, parent):
wx.Button.__init__(self, parent, _NTB_AXISMENU_BUTTON, "Axes: ",
style=wx.BU_EXACTFIT)
self._toolbar = parent
self._menu =wx.Menu()
self._axisId = []
# First two menu items never change...
self._allId =wx.NewId()
self._invertId =wx.NewId()
self._menu.Append(self._allId, "All", "Select all axes", False)
self._menu.Append(self._invertId, "Invert", "Invert axes selected", False)
self._menu.AppendSeparator()
bind(self, wx.EVT_BUTTON, self._onMenuButton, id=_NTB_AXISMENU_BUTTON)
bind(self, wx.EVT_MENU, self._handleSelectAllAxes, id=self._allId)
bind(self, wx.EVT_MENU, self._handleInvertAxesSelected, id=self._invertId)
def Destroy(self):
self._menu.Destroy()
self.Destroy()
def _onMenuButton(self, evt):
"""Handle menu button pressed."""
x, y = self.GetPositionTuple()
w, h = self.GetSizeTuple()
self.PopupMenuXY(self._menu, x, y+h-4)
# When menu returned, indicate selection in button
evt.Skip()
def _handleSelectAllAxes(self, evt):
"""Called when the 'select all axes' menu item is selected."""
if len(self._axisId) == 0:
return
for i in range(len(self._axisId)):
self._menu.Check(self._axisId[i], True)
self._toolbar.set_active(self.getActiveAxes())
evt.Skip()
def _handleInvertAxesSelected(self, evt):
"""Called when the invert all menu item is selected"""
if len(self._axisId) == 0: return
for i in range(len(self._axisId)):
if self._menu.IsChecked(self._axisId[i]):
self._menu.Check(self._axisId[i], False)
else:
self._menu.Check(self._axisId[i], True)
self._toolbar.set_active(self.getActiveAxes())
evt.Skip()
def _onMenuItemSelected(self, evt):
"""Called whenever one of the specific axis menu items is selected"""
current = self._menu.IsChecked(evt.GetId())
if current:
new = False
else:
new = True
self._menu.Check(evt.GetId(), new)
# Lines above would be deleted based on svn tracker ID 2841525;
# not clear whether this matters or not.
self._toolbar.set_active(self.getActiveAxes())
evt.Skip()
def updateAxes(self, maxAxis):
"""Ensures that there are entries for max_axis axes in the menu
(selected by default)."""
if maxAxis > len(self._axisId):
for i in range(len(self._axisId) + 1, maxAxis + 1, 1):
menuId =wx.NewId()
self._axisId.append(menuId)
self._menu.Append(menuId, "Axis %d" % i, "Select axis %d" % i, True)
self._menu.Check(menuId, True)
bind(self, wx.EVT_MENU, self._onMenuItemSelected, id=menuId)
elif maxAxis < len(self._axisId):
for menuId in self._axisId[maxAxis:]:
self._menu.Delete(menuId)
self._axisId = self._axisId[:maxAxis]
self._toolbar.set_active(range(maxAxis))
def getActiveAxes(self):
"""Return a list of the selected axes."""
active = []
for i in range(len(self._axisId)):
if self._menu.IsChecked(self._axisId[i]):
active.append(i)
return active
def updateButtonText(self, lst):
"""Update the list of selected axes in the menu button"""
axis_txt = ''
for e in lst:
axis_txt += '%d,' % (e+1)
# remove trailing ',' and add to button string
self.SetLabel("Axes: %s" % axis_txt[:-1])
cursord = {
cursors.MOVE : wx.CURSOR_HAND,
cursors.HAND : wx.CURSOR_HAND,
cursors.POINTER : wx.CURSOR_ARROW,
cursors.SELECT_REGION : wx.CURSOR_CROSS,
}
class SubplotToolWX(wx.Frame):
def __init__(self, targetfig):
wx.Frame.__init__(self, None, -1, "Configure subplots")
toolfig = Figure((6,3))
canvas = FigureCanvasWx(self, -1, toolfig)
# Create a figure manager to manage things
figmgr = FigureManager(canvas, 1, self)
# Now put all into a sizer
sizer = wx.BoxSizer(wx.VERTICAL)
# This way of adding to sizer allows resizing
sizer.Add(canvas, 1, wx.LEFT|wx.TOP|wx.GROW)
self.SetSizer(sizer)
self.Fit()
tool = SubplotTool(targetfig, toolfig)
class NavigationToolbar2Wx(NavigationToolbar2, wx.ToolBar):
def __init__(self, canvas):
wx.ToolBar.__init__(self, canvas.GetParent(), -1)
NavigationToolbar2.__init__(self, canvas)
self.canvas = canvas
self._idle = True
self.statbar = None
def get_canvas(self, frame, fig):
return FigureCanvasWx(frame, -1, fig)
def _init_toolbar(self):
DEBUG_MSG("_init_toolbar", 1, self)
self._parent = self.canvas.GetParent()
self.wx_ids = {}
for text, tooltip_text, image_file, callback in self.toolitems:
if text is None:
self.AddSeparator()
continue
self.wx_ids[text] = wx.NewId()
if text in ['Pan', 'Zoom']:
self.AddCheckTool(self.wx_ids[text], _load_bitmap(image_file + '.png'),
shortHelp=text, longHelp=tooltip_text)
else:
self.AddSimpleTool(self.wx_ids[text], _load_bitmap(image_file + '.png'),
text, tooltip_text)
bind(self, wx.EVT_TOOL, getattr(self, callback), id=self.wx_ids[text])
self.Realize()
def zoom(self, *args):
self.ToggleTool(self.wx_ids['Pan'], False)
NavigationToolbar2.zoom(self, *args)
def pan(self, *args):
self.ToggleTool(self.wx_ids['Zoom'], False)
NavigationToolbar2.pan(self, *args)
def configure_subplots(self, evt):
frame = wx.Frame(None, -1, "Configure subplots")
toolfig = Figure((6,3))
canvas = self.get_canvas(frame, toolfig)
# Create a figure manager to manage things
figmgr = FigureManager(canvas, 1, frame)
# Now put all into a sizer
sizer = wx.BoxSizer(wx.VERTICAL)
# This way of adding to sizer allows resizing
sizer.Add(canvas, 1, wx.LEFT|wx.TOP|wx.GROW)
frame.SetSizer(sizer)
frame.Fit()
tool = SubplotTool(self.canvas.figure, toolfig)
frame.Show()
def save_figure(self, *args):
# Fetch the required filename and file type.
filetypes, exts, filter_index = self.canvas._get_imagesave_wildcards()
default_file = self.canvas.get_default_filename()
dlg = wx.FileDialog(self._parent, "Save to file", "", default_file,
filetypes,
wx.SAVE|wx.OVERWRITE_PROMPT)
dlg.SetFilterIndex(filter_index)
if dlg.ShowModal() == wx.ID_OK:
dirname = dlg.GetDirectory()
filename = dlg.GetFilename()
DEBUG_MSG('Save file dir:%s name:%s' % (dirname, filename), 3, self)
format = exts[dlg.GetFilterIndex()]
basename, ext = os.path.splitext(filename)
if ext.startswith('.'):
ext = ext[1:]
if ext in ('svg', 'pdf', 'ps', 'eps', 'png') and format!=ext:
#looks like they forgot to set the image type drop
#down, going with the extension.
warnings.warn('extension %s did not match the selected image type %s; going with %s'%(ext, format, ext), stacklevel=0)
format = ext
try:
self.canvas.print_figure(
os.path.join(dirname, filename), format=format)
except Exception as e:
error_msg_wx(str(e))
def set_cursor(self, cursor):
cursor =wx.StockCursor(cursord[cursor])
self.canvas.SetCursor( cursor )
def release(self, event):
try: del self.lastrect
except AttributeError: pass
def dynamic_update(self):
d = self._idle
self._idle = False
if d:
self.canvas.draw()
self._idle = True
def draw_rubberband(self, event, x0, y0, x1, y1):
'adapted from http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/189744'
canvas = self.canvas
dc =wx.ClientDC(canvas)
# Set logical function to XOR for rubberbanding
dc.SetLogicalFunction(wx.XOR)
# Set dc brush and pen
# Here I set brush and pen to white and grey respectively
# You can set it to your own choices
# The brush setting is not really needed since we
# dont do any filling of the dc. It is set just for
# the sake of completion.
wbrush =wx.Brush(wx.Colour(255,255,255), wx.TRANSPARENT)
wpen =wx.Pen(wx.Colour(200, 200, 200), 1, wx.SOLID)
dc.SetBrush(wbrush)
dc.SetPen(wpen)
dc.ResetBoundingBox()
dc.BeginDrawing()
height = self.canvas.figure.bbox.height
y1 = height - y1
y0 = height - y0
if y1 0)
can_forward = (self._views._pos < len(self._views._elements) - 1)
self.EnableTool(self.wx_ids['Back'], can_backward)
self.EnableTool(self.wx_ids['Forward'], can_forward)
class NavigationToolbarWx(wx.ToolBar):
def __init__(self, canvas, can_kill=False):
wx.ToolBar.__init__(self, canvas.GetParent(), -1)
DEBUG_MSG("__init__()", 1, self)
self.canvas = canvas
self._lastControl = None
self._mouseOnButton = None
self._parent = canvas.GetParent()
self._NTB_BUTTON_HANDLER = {
_NTB_X_PAN_LEFT : self.panx,
_NTB_X_PAN_RIGHT : self.panx,
_NTB_X_ZOOMIN : self.zoomx,
_NTB_X_ZOOMOUT : self.zoomy,
_NTB_Y_PAN_UP : self.pany,
_NTB_Y_PAN_DOWN : self.pany,
_NTB_Y_ZOOMIN : self.zoomy,
_NTB_Y_ZOOMOUT : self.zoomy }
self._create_menu()
self._create_controls(can_kill)
self.Realize()
def _create_menu(self):
"""
Creates the 'menu' - implemented as a button which opens a
pop-up menu since wxPython does not allow a menu as a control
"""
DEBUG_MSG("_create_menu()", 1, self)
self._menu = MenuButtonWx(self)
self.AddControl(self._menu)
self.AddSeparator()
def _create_controls(self, can_kill):
"""
Creates the button controls, and links them to event handlers
"""
DEBUG_MSG("_create_controls()", 1, self)
# Need the following line as Windows toolbars default to 15x16
self.SetToolBitmapSize(wx.Size(16,16))
self.AddSimpleTool(_NTB_X_PAN_LEFT, _load_bitmap('stock_left.xpm'),
'Left', 'Scroll left')
self.AddSimpleTool(_NTB_X_PAN_RIGHT, _load_bitmap('stock_right.xpm'),
'Right', 'Scroll right')
self.AddSimpleTool(_NTB_X_ZOOMIN, _load_bitmap('stock_zoom-in.xpm'),
'Zoom in', 'Increase X axis magnification')
self.AddSimpleTool(_NTB_X_ZOOMOUT, _load_bitmap('stock_zoom-out.xpm'),
'Zoom out', 'Decrease X axis magnification')
self.AddSeparator()
self.AddSimpleTool(_NTB_Y_PAN_UP,_load_bitmap('stock_up.xpm'),
'Up', 'Scroll up')
self.AddSimpleTool(_NTB_Y_PAN_DOWN, _load_bitmap('stock_down.xpm'),
'Down', 'Scroll down')
self.AddSimpleTool(_NTB_Y_ZOOMIN, _load_bitmap('stock_zoom-in.xpm'),
'Zoom in', 'Increase Y axis magnification')
self.AddSimpleTool(_NTB_Y_ZOOMOUT, _load_bitmap('stock_zoom-out.xpm'),
'Zoom out', 'Decrease Y axis magnification')
self.AddSeparator()
self.AddSimpleTool(_NTB_SAVE, _load_bitmap('stock_save_as.xpm'),
'Save', 'Save plot contents as images')
self.AddSeparator()
bind(self, wx.EVT_TOOL, self._onLeftScroll, id=_NTB_X_PAN_LEFT)
bind(self, wx.EVT_TOOL, self._onRightScroll, id=_NTB_X_PAN_RIGHT)
bind(self, wx.EVT_TOOL, self._onXZoomIn, id=_NTB_X_ZOOMIN)
bind(self, wx.EVT_TOOL, self._onXZoomOut, id=_NTB_X_ZOOMOUT)
bind(self, wx.EVT_TOOL, self._onUpScroll, id=_NTB_Y_PAN_UP)
bind(self, wx.EVT_TOOL, self._onDownScroll, id=_NTB_Y_PAN_DOWN)
bind(self, wx.EVT_TOOL, self._onYZoomIn, id=_NTB_Y_ZOOMIN)
bind(self, wx.EVT_TOOL, self._onYZoomOut, id=_NTB_Y_ZOOMOUT)
bind(self, wx.EVT_TOOL, self._onSave, id=_NTB_SAVE)
bind(self, wx.EVT_TOOL_ENTER, self._onEnterTool, id=self.GetId())
if can_kill:
bind(self, wx.EVT_TOOL, self._onClose, id=_NTB_CLOSE)
bind(self, wx.EVT_MOUSEWHEEL, self._onMouseWheel)
def set_active(self, ind):
"""
ind is a list of index numbers for the axes which are to be made active
"""
DEBUG_MSG("set_active()", 1, self)
self._ind = ind
if ind != None:
self._active = [ self._axes[i] for i in self._ind ]
else:
self._active = []
# Now update button text wit active axes
self._menu.updateButtonText(ind)
def get_last_control(self):
"""Returns the identity of the last toolbar button pressed."""
return self._lastControl
def panx(self, direction):
DEBUG_MSG("panx()", 1, self)
for a in self._active:
a.xaxis.pan(direction)
self.canvas.draw()
self.canvas.Refresh(eraseBackground=False)
def pany(self, direction):
DEBUG_MSG("pany()", 1, self)
for a in self._active:
a.yaxis.pan(direction)
self.canvas.draw()
self.canvas.Refresh(eraseBackground=False)
def zoomx(self, in_out):
DEBUG_MSG("zoomx()", 1, self)
for a in self._active:
a.xaxis.zoom(in_out)
self.canvas.draw()
self.canvas.Refresh(eraseBackground=False)
def zoomy(self, in_out):
DEBUG_MSG("zoomy()", 1, self)
for a in self._active:
a.yaxis.zoom(in_out)
self.canvas.draw()
self.canvas.Refresh(eraseBackground=False)
def update(self):
"""
Update the toolbar menu - e.g., called when a new subplot
or axes are added
"""
DEBUG_MSG("update()", 1, self)
self._axes = self.canvas.figure.get_axes()
self._menu.updateAxes(len(self._axes))
def _do_nothing(self, d):
"""A NULL event handler - does nothing whatsoever"""
pass
# Local event handlers - mainly supply parameters to pan/scroll functions
def _onEnterTool(self, evt):
toolId = evt.GetSelection()
try:
self.button_fn = self._NTB_BUTTON_HANDLER[toolId]
except KeyError:
self.button_fn = self._do_nothing
evt.Skip()
def _onLeftScroll(self, evt):
self.panx(-1)
evt.Skip()
def _onRightScroll(self, evt):
self.panx(1)
evt.Skip()
def _onXZoomIn(self, evt):
self.zoomx(1)
evt.Skip()
def _onXZoomOut(self, evt):
self.zoomx(-1)
evt.Skip()
def _onUpScroll(self, evt):
self.pany(1)
evt.Skip()
def _onDownScroll(self, evt):
self.pany(-1)
evt.Skip()
def _onYZoomIn(self, evt):
self.zoomy(1)
evt.Skip()
def _onYZoomOut(self, evt):
self.zoomy(-1)
evt.Skip()
def _onMouseEnterButton(self, button):
self._mouseOnButton = button
def _onMouseLeaveButton(self, button):
if self._mouseOnButton == button:
self._mouseOnButton = None
def _onMouseWheel(self, evt):
if evt.GetWheelRotation() > 0:
direction = 1
else:
direction = -1
self.button_fn(direction)
_onSave = NavigationToolbar2Wx.save_figure
def _onClose(self, evt):
self.GetParent().Destroy()
class StatusBarWx(wx.StatusBar):
"""
A status bar is added to _FigureFrame to allow measurements and the
previously selected scroll function to be displayed as a user
convenience.
"""
def __init__(self, parent):
wx.StatusBar.__init__(self, parent, -1)
self.SetFieldsCount(2)
self.SetStatusText("None", 1)
#self.SetStatusText("Measurement: None", 2)
#self.Reposition()
def set_function(self, string):
self.SetStatusText("%s" % string, 1)
#def set_measurement(self, string):
# self.SetStatusText("Measurement: %s" % string, 2)
#< Additions for printing support: Matt Newville
class PrintoutWx(wx.Printout):
"""Simple wrapper around wx Printout class -- all the real work
here is scaling the matplotlib canvas bitmap to the current
printer's definition.
"""
def __init__(self, canvas, width=5.5,margin=0.5, title='matplotlib'):
wx.Printout.__init__(self,title=title)
self.canvas = canvas
# width, in inches of output figure (approximate)
self.width = width
self.margin = margin
def HasPage(self, page):
#current only supports 1 page print
return page == 1
def GetPageInfo(self):
return (1, 1, 1, 1)
def OnPrintPage(self, page):
self.canvas.draw()
dc = self.GetDC()
(ppw,pph) = self.GetPPIPrinter() # printer's pixels per in
(pgw,pgh) = self.GetPageSizePixels() # page size in pixels
(dcw,dch) = dc.GetSize()
(grw,grh) = self.canvas.GetSizeTuple()
# save current figure dpi resolution and bg color,
# so that we can temporarily set them to the dpi of
# the printer, and the bg color to white
bgcolor = self.canvas.figure.get_facecolor()
fig_dpi = self.canvas.figure.dpi
# draw the bitmap, scaled appropriately
vscale = float(ppw) / fig_dpi
# set figure resolution,bg color for printer
self.canvas.figure.dpi = ppw
self.canvas.figure.set_facecolor('#FFFFFF')
renderer = RendererWx(self.canvas.bitmap, self.canvas.figure.dpi)
self.canvas.figure.draw(renderer)
self.canvas.bitmap.SetWidth( int(self.canvas.bitmap.GetWidth() * vscale))
self.canvas.bitmap.SetHeight( int(self.canvas.bitmap.GetHeight()* vscale))
self.canvas.draw()
# page may need additional scaling on preview
page_scale = 1.0
if self.IsPreview(): page_scale = float(dcw)/pgw
# get margin in pixels = (margin in in) * (pixels/in)
top_margin = int(self.margin * pph * page_scale)
left_margin = int(self.margin * ppw * page_scale)
# set scale so that width of output is self.width inches
# (assuming grw is size of graph in inches....)
user_scale = (self.width * fig_dpi * page_scale)/float(grw)
dc.SetDeviceOrigin(left_margin,top_margin)
dc.SetUserScale(user_scale,user_scale)
# this cute little number avoid API inconsistencies in wx
try:
dc.DrawBitmap(self.canvas.bitmap, 0, 0)
except:
try:
dc.DrawBitmap(self.canvas.bitmap, (0, 0))
except:
pass
# restore original figure resolution
self.canvas.figure.set_facecolor(bgcolor)
self.canvas.figure.dpi = fig_dpi
self.canvas.draw()
return True
#>
########################################################################
#
# Now just provide the standard names that backend.__init__ is expecting
#
########################################################################
Toolbar = NavigationToolbarWx
FigureManager = FigureManagerWx
MAVProxy-1.8.74/MAVProxy/modules/lib/MacOS/backend_wxagg.py 0000664 0000000 0000000 00000007234 15043245150 0023310 0 ustar 00root root 0000000 0000000 from __future__ import division, print_function
import matplotlib
from matplotlib.figure import Figure
from matplotlib.backends.backend_agg import FigureCanvasAgg
from matplotlib.backends import backend_wx # already uses wxversion.ensureMinimal('2.8')
from matplotlib.backends.backend_wx import FigureFrameWx
from matplotlib.backends.backend_wx import NavigationToolbar2Wx
from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg
import wx
class FigureFrameWxAgg(FigureFrameWx):
def get_canvas(self, fig):
return FigureCanvasWxAgg(self, -1, fig)
def _get_toolbar(self, statbar):
if matplotlib.rcParams['toolbar']=='classic':
toolbar = NavigationToolbar2Wx(self.canvas, True)
elif matplotlib.rcParams['toolbar']=='toolbar2':
toolbar = NavigationToolbar2WxAgg(self.canvas)
toolbar.set_status_bar(statbar)
else:
toolbar = None
return toolbar
class NavigationToolbar2WxAgg(NavigationToolbar2Wx):
def get_canvas(self, frame, fig):
return FigureCanvasWxAgg(frame, -1, fig)
def new_figure_manager(num, *args, **kwargs):
"""
Create a new figure manager instance
"""
# in order to expose the Figure constructor to the pylab
# interface we need to create the figure here
backend_wx._create_wx_app()
FigureClass = kwargs.pop('FigureClass', Figure)
fig = FigureClass(*args, **kwargs)
return new_figure_manager_given_figure(num, fig)
def new_figure_manager_given_figure(num, figure):
"""
Create a new figure manager instance for the given figure.
"""
frame = FigureFrameWxAgg(num, figure)
figmgr = frame.get_figure_manager()
if matplotlib.is_interactive():
figmgr.frame.Show()
return figmgr
#
# agg/wxPython image conversion functions (wxPython >= 2.8)
#
def _convert_agg_to_wx_image(agg, bbox):
"""
Convert the region of the agg buffer bounded by bbox to a wx.Image. If
bbox is None, the entire buffer is converted.
Note: agg must be a backend_agg.RendererAgg instance.
"""
if bbox is None:
# agg => rgb -> image
image = wx.EmptyImage(int(agg.width), int(agg.height))
image.SetData(agg.tostring_rgb())
return image
else:
# agg => rgba buffer -> bitmap => clipped bitmap => image
return wx.ImageFromBitmap(_WX28_clipped_agg_as_bitmap(agg, bbox))
def _convert_agg_to_wx_bitmap(agg, bbox):
"""
Convert the region of the agg buffer bounded by bbox to a wx.Bitmap. If
bbox is None, the entire buffer is converted.
Note: agg must be a backend_agg.RendererAgg instance.
"""
if bbox is None:
# agg => rgba buffer -> bitmap
return wx.BitmapFromBufferRGBA(int(agg.width), int(agg.height),
agg.buffer_rgba())
else:
# agg => rgba buffer -> bitmap => clipped bitmap
return _WX28_clipped_agg_as_bitmap(agg, bbox)
def _WX28_clipped_agg_as_bitmap(agg, bbox):
"""
Convert the region of a the agg buffer bounded by bbox to a wx.Bitmap.
Note: agg must be a backend_agg.RendererAgg instance.
"""
l, b, width, height = bbox.bounds
r = l + width
t = b + height
srcBmp = wx.BitmapFromBufferRGBA(int(agg.width), int(agg.height),
agg.buffer_rgba())
srcDC = wx.MemoryDC()
srcDC.SelectObject(srcBmp)
destBmp = wx.EmptyBitmap(int(width), int(height))
destDC = wx.MemoryDC()
destDC.SelectObject(destBmp)
destDC.BeginDrawing()
x = int(l)
y = int(int(agg.height) - t)
destDC.Blit(0, 0, int(width), int(height), srcDC, x, y)
destDC.EndDrawing()
srcDC.SelectObject(wx.NullBitmap)
destDC.SelectObject(wx.NullBitmap)
return destBmp MAVProxy-1.8.74/MAVProxy/modules/lib/MacOS/wxversion.py 0000664 0000000 0000000 00000042666 15043245150 0022600 0 ustar 00root root 0000000 0000000 #----------------------------------------------------------------------
# Name: wxversion
# Purpose: Allows a wxPython program to search for alternate
# installations of the wxPython packages and modify sys.path
# so they will be found when "import wx" is done.
#
# Author: Robin Dunn
#
# Created: 24-Sept-2004
# RCS-ID: $Id$
# Copyright: (c) 2004 by Total Control Software
# Licence: wxWindows license
#----------------------------------------------------------------------
"""
If you have more than one version of wxPython installed this module
allows your application to choose which version of wxPython will be
imported when it does 'import wx'. The main function of this module
is `select` and you use it like this::
import wxversion
wxversion.select('2.4')
import wx
Or additional build options can also be selected, although they will
not be required if they are not installed, like this::
import wxversion
wxversion.select('2.5.3-unicode')
import wx
Or you can require an exact match on the build options like this::
import wxversion
wxversion.select('2.5.3-unicode', optionsRequired=True)
import wx
Finally you can also specify a collection of versions that are allowed
by your application, like this::
import wxversion
wxversion.select(['2.5.4', '2.5.5', '2.6'])
import wx
Of course the default wxPython version can also be controlled by
setting PYTHONPATH or by editing the wx.pth path configuration file,
but using wxversion will allow an application to manage the version
selection itself rather than depend on the user to setup the
environment correctly.
It works by searching the sys.path for directories matching wx-* and
then comparing them to what was passed to the select function. If a
match is found then that path is inserted into sys.path.
NOTE: If you are making a 'bundle' of your application with a tool
like py2exe then you should *not* use the wxversion module since it
looks at the filesystem for the directories on sys.path, it will fail
in a bundled environment. Instead you should simply ensure that the
version of wxPython that you want is found by default on the sys.path
when making the bundled version by setting PYTHONPATH. Then that
version will be included in your bundle and your app will work as
expected. Py2exe and the others usually have a way to tell at runtime
if they are running from a bundle or running raw, so you can check
that and only use wxversion if needed. For example, for py2exe::
if not hasattr(sys, 'frozen'):
import wxversion
wxversion.select('2.5')
import wx
More documentation on wxversion and multi-version installs can be
found at: http://wiki.wxpython.org/index.cgi/MultiVersionInstalls
"""
import re, sys, os, glob, fnmatch
_selected = None
class VersionError(Exception):
pass
class AlreadyImportedError(VersionError):
pass
#----------------------------------------------------------------------
def select(versions, optionsRequired=False):
"""
Search for a wxPython installation that matches version. If one
is found then sys.path is modified so that version will be
imported with a 'import wx', otherwise a VersionError exception is
raised. This function should only be called once at the beginning
of the application before wxPython is imported.
:param versions: Specifies the version to look for, it can
either be a string or a list of strings. Each string is
compared to the installed wxPythons and the best match is
inserted into the sys.path, allowing an 'import wx' to
find that version.
The version string is composed of the dotted version
number (at least 2 of the 4 components) optionally
followed by hyphen ('-') separated options (wx port,
unicode/ansi, flavour, etc.) A match is determined by how
much of the installed version matches what is given in the
version parameter. If the version number components don't
match then the score is zero, otherwise the score is
increased for every specified optional component that is
specified and that matches.
Please note, however, that it is possible for a match to
be selected that doesn't exactly match the versions
requested. The only component that is required to be
matched is the version number. If you need to require a
match on the other components as well, then please use the
optional ``optionsRequired`` parameter described next.
:param optionsRequired: Allows you to specify that the other
components of the version string (such as the port name
or character type) are also required to be present for an
installed version to be considered a match. Using this
parameter allows you to change the selection from a soft,
as close as possible match to a hard, exact match.
"""
if type(versions) == str:
versions = [versions]
global _selected
if _selected is not None:
# A version was previously selected, ensure that it matches
# this new request
for ver in versions:
if _selected.Score(_wxPackageInfo(ver), optionsRequired) > 0:
return
# otherwise, raise an exception
raise VersionError("A previously selected wx version does not match the new request.")
# If we get here then this is the first time wxversion is used,
# ensure that wxPython hasn't been imported yet.
if sys.modules.has_key('wx') or sys.modules.has_key('wxPython'):
raise AlreadyImportedError("wxversion.select() must be called before wxPython is imported")
# Look for a matching version and manipulate the sys.path as
# needed to allow it to be imported.
installed = _find_installed(True)
bestMatch = _get_best_match(installed, versions, optionsRequired)
if bestMatch is None:
raise VersionError("Requested version of wxPython not found")
sys.path.insert(0, bestMatch.pathname)
# q.v. Bug #1409256
path64 = re.sub('/lib/','/lib64/',bestMatch.pathname)
if os.path.isdir(path64):
sys.path.insert(0, path64)
_selected = bestMatch
#----------------------------------------------------------------------
UPDATE_URL = "http://wxPython.org/"
#UPDATE_URL = "http://sourceforge.net/project/showfiles.php?group_id=10718"
_EM_DEBUG=0
def ensureMinimal(minVersion, optionsRequired=False):
"""
Checks to see if the default version of wxPython is greater-than
or equal to `minVersion`. If not then it will try to find an
installed version that is >= minVersion. If none are available
then a message is displayed that will inform the user and will
offer to open their web browser to the wxPython downloads page,
and will then exit the application.
"""
assert type(minVersion) == str
# ensure that wxPython hasn't been imported yet.
if sys.modules.has_key('wx') or sys.modules.has_key('wxPython'):
raise AlreadyImportedError("wxversion.ensureMinimal() must be called before wxPython is imported")
bestMatch = None
minv = _wxPackageInfo(minVersion)
# check the default version first
defaultPath = _find_default()
if defaultPath:
defv = _wxPackageInfo(defaultPath, True)
if defv >= minv and minv.CheckOptions(defv, optionsRequired):
bestMatch = defv
# if still no match then check look at all installed versions
if bestMatch is None:
installed = _find_installed()
# The list is in reverse sorted order, so find the first
# one that is big enough and optionally matches the
# options
for inst in installed:
if inst >= minv and minv.CheckOptions(inst, optionsRequired):
bestMatch = inst
break
# if still no match then prompt the user
if bestMatch is None:
if _EM_DEBUG: # We'll do it this way just for the test code below
raise VersionError("Requested version of wxPython not found")
import wx, webbrowser
versions = "\n".join([" "+ver for ver in getInstalled()])
app = wx.App()
result = wx.MessageBox("This application requires a version of wxPython "
"greater than or equal to %s, but a matching version "
"was not found.\n\n"
"You currently have these version(s) installed:\n%s\n\n"
"Would you like to download a new version of wxPython?\n"
% (minVersion, versions),
"wxPython Upgrade Needed", style=wx.YES_NO)
if result == wx.YES:
webbrowser.open(UPDATE_URL)
app.MainLoop()
sys.exit()
sys.path.insert(0, bestMatch.pathname)
# q.v. Bug #1409256
path64 = re.sub('/lib/','/lib64/',bestMatch.pathname)
if os.path.isdir(path64):
sys.path.insert(0, path64)
global _selected
_selected = bestMatch
#----------------------------------------------------------------------
def checkInstalled(versions, optionsRequired=False):
"""
Check if there is a version of wxPython installed that matches one
of the versions given. Returns True if so, False if not. This
can be used to determine if calling `select` will succeed or not.
:param versions: Same as in `select`, either a string or a list
of strings specifying the version(s) to check for.
:param optionsRequired: Same as in `select`.
"""
if type(versions) == str:
versions = [versions]
installed = _find_installed()
bestMatch = _get_best_match(installed, versions, optionsRequired)
return bestMatch is not None
#----------------------------------------------------------------------
def getInstalled():
"""
Returns a list of strings representing the installed wxPython
versions that are found on the system.
"""
installed = _find_installed()
return [os.path.basename(p.pathname)[3:] for p in installed]
#----------------------------------------------------------------------
# private helpers...
def _get_best_match(installed, versions, optionsRequired):
bestMatch = None
bestScore = 0
for pkg in installed:
for ver in versions:
score = pkg.Score(_wxPackageInfo(ver), optionsRequired)
if score > bestScore:
bestMatch = pkg
bestScore = score
return bestMatch
_pattern = "wx-[0-9].*"
def _find_installed(removeExisting=False):
installed = []
toRemove = []
for pth in sys.path:
# empty means to look in the current dir
if not pth:
pth = '.'
# skip it if it's not a package dir
if not os.path.isdir(pth):
continue
base = os.path.basename(pth)
# if it's a wx path that's already in the sys.path then mark
# it for removal and then skip it
if fnmatch.fnmatchcase(base, _pattern):
toRemove.append(pth)
continue
# now look in the dir for matching subdirs
for name in glob.glob(os.path.join(pth, _pattern)):
# make sure it's a directory
if not os.path.isdir(name):
continue
# and has a wx subdir
if not os.path.exists(os.path.join(name, 'wx')):
continue
installed.append(_wxPackageInfo(name, True))
if removeExisting:
for rem in toRemove:
del sys.path[sys.path.index(rem)]
installed.sort()
installed.reverse()
return installed
# Scan the sys.path looking for either a directory matching _pattern,
# or a wx.pth file
def _find_default():
for pth in sys.path:
# empty means to look in the current dir
if not pth:
pth = '.'
# skip it if it's not a package dir
if not os.path.isdir(pth):
continue
# does it match the pattern?
base = os.path.basename(pth)
if fnmatch.fnmatchcase(base, _pattern):
return pth
for pth in sys.path:
if not pth:
pth = '.'
if not os.path.isdir(pth):
continue
if os.path.exists(os.path.join(pth, 'wx.pth')):
base = open(os.path.join(pth, 'wx.pth')).read()
return os.path.join(pth, base)
return None
class _wxPackageInfo(object):
def __init__(self, pathname, stripFirst=False):
self.pathname = pathname
base = os.path.basename(pathname)
segments = base.split('-')
if stripFirst:
segments = segments[1:]
self.version = tuple([int(x) for x in segments[0].split('.')])
self.options = segments[1:]
def Score(self, other, optionsRequired):
score = 0
# whatever number of version components given in other must
# match exactly
minlen = min(len(self.version), len(other.version))
if self.version[:minlen] != other.version[:minlen]:
return 0
score += 1
# check for matching options, if optionsRequired then the
# options are not optional ;-)
for opt in other.options:
if opt in self.options:
score += 1
elif optionsRequired:
return 0
return score
def CheckOptions(self, other, optionsRequired):
# if options are not required then this always succeeds
if not optionsRequired:
return True
# otherwise, if we have any option not present in other, then
# the match fails.
for opt in self.options:
if opt not in other.options:
return False
return True
def __lt__(self, other):
return self.version < other.version or \
(self.version == other.version and self.options < other.options)
def __le__(self, other):
return self.version <= other.version or \
(self.version == other.version and self.options <= other.options)
def __gt__(self, other):
return self.version > other.version or \
(self.version == other.version and self.options > other.options)
def __ge__(self, other):
return self.version >= other.version or \
(self.version == other.version and self.options >= other.options)
def __eq__(self, other):
return self.version == other.version and self.options == other.options
#----------------------------------------------------------------------
if __name__ == '__main__':
import pprint
#ensureMinimal('2.5')
#pprint.pprint(sys.path)
#sys.exit()
def test(version, optionsRequired=False):
# setup
savepath = sys.path[:]
#test
select(version, optionsRequired)
print("Asked for %s, (%s):\t got: %s" % (version, optionsRequired, sys.path[0]))
# reset
sys.path = savepath[:]
global _selected
_selected = None
def testEM(version, optionsRequired=False):
# setup
savepath = sys.path[:]
#test
ensureMinimal(version, optionsRequired)
print("EM: Asked for %s, (%s):\t got: %s" % (version, optionsRequired, sys.path[0]))
# reset
sys.path = savepath[:]
global _selected
_selected = None
# make some test dirs
names = ['wx-2.4-gtk-ansi',
'wx-2.5.2-gtk2-unicode',
'wx-2.5.3-gtk-ansi',
'wx-2.6-gtk2-unicode',
'wx-2.6-gtk2-ansi',
'wx-2.6-gtk-ansi',
'wx-2.7.1-gtk2-ansi',
]
for name in names:
d = os.path.join('/tmp', name)
os.mkdir(d)
os.mkdir(os.path.join(d, 'wx'))
# setup sys.path to see those dirs
sys.path.append('/tmp')
# now run some tests
pprint.pprint( getInstalled())
print(checkInstalled("2.4"))
print(checkInstalled("2.5-unicode"))
print(checkInstalled("2.99-bogus"))
print("Current sys.path:")
pprint.pprint(sys.path)
print("")
test("2.4")
test("2.5")
test("2.5-gtk2")
test("2.5.2")
test("2.5-ansi")
test("2.5-unicode")
test("2.6")
test("2.6-ansi")
test(["2.6-unicode", "2.7-unicode"])
test(["2.6", "2.7"])
test(["2.6-unicode", "2.7-unicode"], optionsRequired=True)
# There isn't a unicode match for this one, but it will give the best
# available 2.4. Should it give an error instead? I don't think so...
test("2.4-unicode")
# Try asking for multiple versions
test(["2.5.2", "2.5.3", "2.6"])
try:
# expecting an error on this one
test("2.9")
except VersionError as e:
print("Asked for 2.9:\t got Exception:", e)
# check for exception when incompatible versions are requested
try:
select("2.4")
select("2.5")
except VersionError as e:
print("Asked for incompatible versions, got Exception:", e)
_EM_DEBUG=1
testEM("2.6")
testEM("2.6-unicode")
testEM("2.6-unicode", True)
try:
testEM("2.9")
except VersionError as e:
print("EM: Asked for 2.9:\t got Exception:", e)
# cleanup
for name in names:
d = os.path.join('/tmp', name)
os.rmdir(os.path.join(d, 'wx'))
os.rmdir(d)
MAVProxy-1.8.74/MAVProxy/modules/lib/__init__.py 0000664 0000000 0000000 00000000021 15043245150 0021304 0 ustar 00root root 0000000 0000000 # Allow imports.
MAVProxy-1.8.74/MAVProxy/modules/lib/camera_projection.py 0000775 0000000 0000000 00000040636 15043245150 0023254 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
class to project a camera view onto the map
'''
import numpy
from numpy import array, eye, zeros, uint64
from numpy import linalg, dot, transpose
from numpy import sin, cos, pi
from math import pi
import math
import cv2
from MAVProxy.modules.lib import mp_elevation
from pymavlink.rotmat import Vector3
from MAVProxy.modules.lib import mp_util
import time
class CameraParams:
'''
object representing camera parameters. Can be initialised from basic parameters for idealised lens or use fromfile() to loa
from a calibration file
'''
def __init__(self, lens=None, sensorwidth=None, xresolution=None, yresolution=None, K=None, D=None, FOV=None):
if xresolution is None:
raise ValueError("xresolution required")
if yresolution is None:
raise ValueError("yresolution required")
if lens is None and FOV is not None:
# FOV = 2 arctan(xres/(2f))
if sensorwidth is None:
# default to 5mm sensor width for FOV calculation
sensorwidth=5.0 # mm
# FOV = 2 arctan( sensorwidth/(2f))
lens = 0.5 * sensorwidth / math.tan(math.radians(FOV)*0.5)
if lens is None:
raise ValueError("Lens required")
if sensorwidth is None:
raise ValueError("sensorwidth required")
self.version = 0
self.sensorwidth = sensorwidth
self.lens = lens
self.K = K
self.D = D
self.set_resolution(xresolution, yresolution)
self.FOV = FOV
if FOV is None:
self.FOV = math.degrees(2*math.atan(sensorwidth/(2*lens)))
def set_resolution(self, xresolution, yresolution):
'''set camera resolution'''
self.xresolution = xresolution
self.yresolution = yresolution
# compute focal length in pixels
f_p = xresolution * self.lens / self.sensorwidth
if self.K is None:
self.K = array([[f_p, 0.0, xresolution/2],[0.0, f_p, yresolution/2], [0.0,0.0,1.0]])
if self.D is None:
self.D = array([[0.0, 0.0, 0.0, 0.0, 0.0]])
def __repr__(self):
return json.dumps(self.todict(),indent=2)
def setParams(self, K, D):
self.K = array(K)
self.D = array(D)
def todict(self):
data = {}
data['version'] = self.version
data['lens'] = self.lens
data['sensorwidth'] = self.sensorwidth
data['xresolution'] = self.xresolution
data['yresolution'] = self.yresolution
if self.K is not None:
data['K'] = self.K.tolist()
print("Set K to " + str(self.K))
print("Set K to " + str(data['K']))
if self.D is not None:
data['D'] = self.D.tolist()
return data
@staticmethod
def fromdict(data):
if data['version'] != 0:
raise Exception('version %d of camera params unsupported' % (self.version))
try:
K = array(data['K'])
D = array(data['D'])
except KeyError:
K = None
D = None
ret = CameraParams(lens=data['lens'],
sensorwidth=data['sensorwidth'],
xresolution=data['xresolution'],
yresolution=data['yresolution'],
K=K,
D=D)
ret.version = data['version']
return ret;
@staticmethod
def fromstring(strung):
dic = json.loads(str(strung))
return CameraParams.fromdict(dic)
def save(self, filename):
f = open(filename,"w")
# dump json form
f.write(str(self)+"\n")
f.close()
@staticmethod
def fromfile(filename):
f = open(filename,"r")
# dump json form
d = f.read(65535)
f.close()
return CameraParams.fromstring(d)
# a simple frame transformation matrix
def rotationMatrix(phi, theta, psi):
R_phi = array([[1.0,0.0,0.0],[0.0,cos(phi),sin(phi)],[0.0,-sin(phi),cos(phi)]])
R_theta = array([[ cos(theta),0.0,-sin(theta)],[0.0,1.0,0.0],[ sin(theta),0.0,cos(theta)]])
R_psi = array([[cos(psi),sin(psi),0.0],[-sin(psi),cos(psi),0.0],[0.0,0.0,1.0]])
R = dot(dot(R_phi,R_theta),R_psi)
return R
class uavxfer:
'''
camera transfer function implementation, thanks to Matt Ridley from CanberraUAV
'''
def setCameraParams(self, fu, fv, cu, cv):
K = array([[fu, 0.0, cu],[0.0, fv, cv],[0.0, 0.0, 1.0]])
self.setCameraMatrix(K)
def setCameraMatrix(self, K):
K_i = linalg.inv(K)
self.Tk = eye(4,4)
self.Tk[:3,:3] = K;
self.Tk_i = eye(4,4)
self.Tk_i[:3,:3] = K_i
def setCameraOrientation(self, roll, pitch, yaw):
self.Rc = array(eye(4,4))
self.Rc[:3,:3] = transpose(rotationMatrix(roll, pitch, yaw))
self.Rc_i = linalg.inv(self.Rc)
def setPlatformPose(self, north, east, down, roll, pitch, yaw):
'''set pose, angles in degrees, 0,0,0 is straight down'''
self.Xp = array([north, east, down, 1.0])
self.Rp = array(eye(4,4))
self.Rp[:3,:3] = transpose(rotationMatrix(roll, pitch, yaw))
self.Rp[:3,3] = array([north, east, down])
self.Rp_i = linalg.inv(self.Rp)
def setFlatEarth(self, z):
self.z_earth = z
def worldToPlatform(self, north, east, down):
x_w = array([north, east, down, 1.0])
x_p = dot(self.Rp_i, x_w)[:3]
return x_p
def worldToImage(self, north, east, down):
x_w = array([north, east, down, 1.0])
x_p = dot(self.Rp_i, x_w)
x_c = dot(self.Rc_i, x_p)
x_i = dot(self.Tk, x_c)
return x_i[:3]/x_i[2]
def platformToWorld(self, north, east, down):
x_p = array([north, east, down, 1.0])
x_w = dot(self.Rp, x_p)
return x_w
def imageToWorld(self, u, v):
x_i = array([u, v, 1.0, 0.0])
v_c = dot(self.Tk_i, x_i)
v_p = dot(self.Rc, v_c)
v_w = dot(self.Rp, v_p)
# compute scale for z == z_earth
scale = (self.z_earth-self.Xp[2])/v_w[2]
#project from platform to ground
x_w = scale*v_w + self.Xp;
return x_w, scale
def __init__(self, fu=200, fv=200, cu=512, cv=480):
self.setCameraParams(fu, fv, cu, cv)
self.Rc = self.Rc_i = array(eye(4,4))
self.Rp = self.Rp_i = array(eye(4,4))
self.z_earth = -600
class CameraProjection:
def __init__(self, C, elevation_model=None, terrain_source="SRTM3"):
self.C = C
self.elevation_model = elevation_model
if elevation_model is None:
self.elevation_model = mp_elevation.ElevationModel(database=terrain_source)
def pixel_position_flat(self, xpos, ypos, height_agl, roll_deg, pitch_deg, yaw_deg):
'''
find the NED offset on the ground in meters of a pixel in a ground image
given height above the ground in meters, and pitch/roll/yaw in degrees, the
lens and image parameters
The xpos,ypos is from the top-left of the image
The height_agl is in meters above ground level. Flat earth below camera is assumed
The yaw is from grid north. Positive yaw is clockwise
The roll is from horiznotal. Positive roll is down on the right
The pitch is from horiznotal. Positive pitch is up in the front, -90 is straight down
return result is a tuple, with meters north, east and down of current GPS position
'''
xfer = uavxfer()
xfer.setCameraMatrix(self.C.K)
xfer.setCameraOrientation( 0.0, 0.0, pi/2 )
xfer.setFlatEarth(0);
xfer.setPlatformPose(0, 0, -height_agl, math.radians(roll_deg), math.radians(pitch_deg+90), math.radians(yaw_deg))
# compute the undistorted points for the ideal camera matrix
src = numpy.zeros((1,1,2), numpy.float32)
src[0,0] = (xpos, ypos)
R = eye(3)
K = self.C.K
D = self.C.D
dst = cv2.undistortPoints(src, K, D, R, K)
x = dst[0,0][0]
y = dst[0,0][1]
# negative scale means camera pointing above horizon
# large scale means a long way away also unreliable
(pos_w, scale) = xfer.imageToWorld(x, y)
if scale < 0:
return None
ret = Vector3(pos_w[0], pos_w[1], height_agl)
return ret
def get_posned(self, x, y, clat, clon, calt_amsl, roll_deg, pitch_deg, yaw_deg):
'''
get a Vector3() NED from the camera position to project onto the ground
return pos NED from camera as Vector3 or None
'''
# get height of terrain below camera
theight = self.elevation_model.GetElevation(clat, clon)
if calt_amsl <= theight:
return None
# project with flat earth
pos_ned = self.pixel_position_flat(x, y, calt_amsl-theight, roll_deg, pitch_deg, yaw_deg)
if pos_ned is None or pos_ned.z <= 0:
return None
# iterate to make more accurate, accounting for difference in terrain height at this point
latlon = mp_util.gps_offset(clat, clon, pos_ned.y, pos_ned.x)
for i in range(3):
ground_alt = self.elevation_model.GetElevation(latlon[0], latlon[1])
if ground_alt is None:
return None
sr = pos_ned.length()
if sr <= 1:
return None
posd2 = calt_amsl - ground_alt
sin_pitch = pos_ned.z / sr
# adjust for height at this point
sr2 = sr - (pos_ned.z - posd2) / sin_pitch
#print("SR: ", pos_ned.z, posd2, sr, sr2)
pos_ned = pos_ned * (sr2 / sr)
latlon = mp_util.gps_offset(clat, clon, pos_ned.y, pos_ned.x)
if latlon is None:
return None
return pos_ned
def get_latlonalt_for_pixel(self, x, y, clat, clon, calt_amsl, roll_deg, pitch_deg, yaw_deg):
'''
get lat,lon of projected pixel from camera.
x,y are pixel coordinates, 0,0 is top-left corner
return is (lat,lon,alt) tuple or None
'''
pos_ned = self.get_posned(x, y, clat, clon, calt_amsl, roll_deg, pitch_deg, yaw_deg)
if pos_ned is None or pos_ned.z <= 0:
return None
latlon = mp_util.gps_offset(clat, clon, pos_ned.y, pos_ned.x)
if latlon is None:
return None
return (latlon[0], latlon[1], calt_amsl-pos_ned.z)
def get_slantrange(self, clat, clon, calt_amsl, roll_deg, pitch_deg, yaw_deg):
'''
get slant range to ground for a pixel in the middle of the camera view
return is slant range in meters or None
'''
pos_ned = self.get_posned(self.C.xresolution//2, self.C.yresolution//2, clat, clon, calt_amsl, roll_deg, pitch_deg, yaw_deg)
if pos_ned is None:
return None
return pos_ned.length()
def get_projection(self, clat, clon, calt_amsl, roll_deg, pitch_deg, yaw_deg):
'''return a list of (lat,lon) tuples drawing the camera view on the terrain'''
ret = []
xres = self.C.xresolution
yres = self.C.yresolution
for (x,y) in [(0,0), (xres, 0), (xres, yres), (0,yres)]:
y0 = 0
while True:
latlonalt = self.get_latlonalt_for_pixel(x,y+y0,clat,clon,calt_amsl,roll_deg,pitch_deg,yaw_deg)
if latlonalt is not None:
break
# chop off the top 10 pixels and try again
y0 += 10
if y0 >= self.C.yresolution:
# give up
return None
ret.append((latlonalt[0],latlonalt[1]))
ret.append(ret[0])
return ret
def test_pixel_position():
C = CameraParams(lens=4.0, sensorwidth=5.0, xresolution=1024, yresolution=768)
cproj = CameraProjection(C)
print("FOV: %.1f degrees" % C.FOV)
pos_ned = cproj.pixel_position_flat(100, 100, 123, 2, -89.9, 0)
assert abs((pos_ned - Vector3(43.6719, -67.3798, 123)).length()) < 0.01
pos_ned = cproj.pixel_position_flat(0, 130, 57, 2, -89.9, 0)
assert abs((pos_ned - Vector3(18.188, -38.4761, 57)).length()) < 0.01
if __name__ == "__main__":
C1 = CameraParams(xresolution=640, yresolution=512, FOV=24.2)
print("Lens: %.2f" % C1.lens)
C2 = CameraParams(xresolution=2560, yresolution=1440, FOV=88.0)
print("Lens: %.2f" % C2.lens)
test_pixel_position()
from argparse import ArgumentParser
parser = ArgumentParser("camera_projection.py [options]")
parser.add_argument("--lat", type=float, default=-35.363261, help="start latitude")
parser.add_argument("--lon", type=float, default=149.165230, help="start longitude")
parser.add_argument("--alt-agl", type=float, default=100.0, help="height AGL")
parser.add_argument("--roll", type=float, default=0.0, help="roll")
parser.add_argument("--pitch", type=float, default=-30, help="pitch")
parser.add_argument("--yaw", type=float, default=0.0, help="yaw")
parser.add_argument("--yaw-delta", type=float, default=0.0, help="yaw delta per loop")
parser.add_argument("--pitch-delta", type=float, default=-1.0, help="pitch delta per loop")
parser.add_argument("--service", default="MicrosoftSat", help="tile service")
parser.add_argument("--offline", action='store_true', default=False, help="no download")
parser.add_argument("--delay", type=float, default=0.3, help="tile download delay")
parser.add_argument("--max-zoom", type=int, default=19, help="maximum tile zoom")
parser.add_argument("--debug", action='store_true', default=False, help="show debug info")
parser.add_argument("--boundary", default=None, help="show boundary")
parser.add_argument("--mission", default=[], action='append', help="show mission")
parser.add_argument("--thumbnail", default=None, help="show thumbnail")
parser.add_argument("--icon", default=None, help="show icon")
parser.add_argument("--flag", default=[], type=str, action='append', help="flag positions")
parser.add_argument("--grid", default=False, action='store_true', help="add a UTM grid")
parser.add_argument("--verbose", action='store_true', default=False, help="show mount actions")
parser.add_argument("--terrain-source", type=str, default="SRTM1", choices=["SRTM1", "SRTM3", "None"], help="Elevation model")
args = parser.parse_args()
from MAVProxy.modules.mavproxy_map import mp_slipmap
sm = mp_slipmap.MPSlipMap(lat=args.lat,
lon=args.lon,
download=not args.offline,
service=args.service,
debug=args.debug,
max_zoom=args.max_zoom,
elevation=args.terrain_source,
tile_delay=args.delay)
lat = args.lat
lon = args.lon
pitch = args.pitch
pitch_delta = args.pitch_delta
yaw = args.yaw
yaw_delta = args.yaw_delta
elevation_model = mp_elevation.ElevationModel(database=args.terrain_source)
alt_amsl = args.alt_agl + elevation_model.GetElevation(lat, lon,timeout=10)
print("Camera alt: %.1f" % alt_amsl)
C1 = CameraParams(xresolution=2560, yresolution=1440, FOV=88.0)
C2 = CameraParams(xresolution=640, yresolution=512, FOV=24.2)
cproj1 = CameraProjection(C1, elevation_model=elevation_model)
cproj2 = CameraProjection(C2, elevation_model=elevation_model)
while sm.is_alive():
p1 = cproj1.get_projection(lat, lon, alt_amsl, args.roll, pitch, yaw)
if p1 is not None:
sm.add_object(mp_slipmap.SlipPolygon('projection1', p1, layer=1, linewidth=2, colour=(0,255,0)))
p2 = cproj2.get_projection(lat, lon, alt_amsl, args.roll, pitch, yaw)
if p2 is not None:
sm.add_object(mp_slipmap.SlipPolygon('projection2', p2, layer=1, linewidth=2, colour=(0,0,255)))
pitch += pitch_delta
if pitch < -180 - args.pitch:
pitch_delta *= -1.0
pitch = -180 - args.pitch
if pitch > args.pitch:
pitch_delta *= -1.0
pitch = args.pitch
yaw += yaw_delta
if yaw < 0:
yaw += 360
if yaw > 360:
yaw -= 360
while not sm.event_queue_empty():
obj = sm.get_event()
if isinstance(obj, mp_slipmap.SlipMouseEvent):
lat = obj.latlon[0]
lon = obj.latlon[1]
alt_amsl = args.alt_agl + elevation_model.GetElevation(lat, lon,timeout=10)
print("Pitch %.1f Yaw %.1f" % (pitch, yaw))
time.sleep(0.1)
MAVProxy-1.8.74/MAVProxy/modules/lib/dumpstacks.py 0000664 0000000 0000000 00000001654 15043245150 0021740 0 ustar 00root root 0000000 0000000
import threading, sys, traceback
# Import dumpstacks to install a SIGQUIT handler that shows a stack dump for all stacks
# From http://stackoverflow.com/questions/132058/showing-the-stack-trace-from-a-running-python-application
def dumpstacks(signal, frame):
id2name = dict([(th.ident, th.name) for th in threading.enumerate()])
code = []
for threadId, stack in sys._current_frames().items():
code.append("\n# Thread: %s(%d)" % (id2name.get(threadId, ""), threadId))
for filename, lineno, name, line in traceback.extract_stack(stack):
code.append('File: "%s", line %d, in %s' % (filename, lineno, name))
if line:
code.append(" %s" % (line.strip()))
print("\n".join(code))
try:
import signal
signal.signal(signal.SIGQUIT, dumpstacks)
except Exception as e:
# Silently ignore failures installing this handler (probably won't work on Windows)
pass
MAVProxy-1.8.74/MAVProxy/modules/lib/geodesic_grid.py 0000664 0000000 0000000 00000023613 15043245150 0022350 0 ustar 00root root 0000000 0000000 # Copyright (C) 2016 Intel Corporation. All rights reserved.
#
# This file is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by the
# Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This file is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
# See the GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program. If not, see .
'''
This module takes
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/AP_GeodesicGrid.h
and
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Math/AP_GeodesicGrid.cpp
as reference for defining the geodesic sections and implementing almost all
functions. Those files should be consulted for implementation details.
'''
import math
from pymavlink.rotmat import Matrix3, Vector3
# The golden number below was obtained from scipy.constants.golden. Let's use
# the literal value here as this is the only place that would require scipy
# module.
g = 1.618033988749895
_first_half = (
(Vector3(-g, 1, 0), Vector3(-1, 0,-g), Vector3(-g,-1, 0)),
(Vector3(-1, 0,-g), Vector3(-g,-1, 0), Vector3( 0,-g,-1)),
(Vector3(-g,-1, 0), Vector3( 0,-g,-1), Vector3( 0,-g, 1)),
(Vector3(-1, 0,-g), Vector3( 0,-g,-1), Vector3( 1, 0,-g)),
(Vector3( 0,-g,-1), Vector3( 0,-g, 1), Vector3( g,-1, 0)),
(Vector3( 0,-g,-1), Vector3( 1, 0,-g), Vector3( g,-1, 0)),
(Vector3( g,-1, 0), Vector3( 1, 0,-g), Vector3( g, 1, 0)),
(Vector3( 1, 0,-g), Vector3( g, 1, 0), Vector3( 0, g,-1)),
(Vector3( 1, 0,-g), Vector3( 0, g,-1), Vector3(-1, 0,-g)),
(Vector3( 0, g,-1), Vector3(-g, 1, 0), Vector3(-1, 0,-g)),
)
_second_half = tuple((-a, -b, -c) for a, b, c in _first_half)
triangles = _first_half + _second_half
radius = math.sqrt(1 + g**2)
# radius / (length of two vertices of an icosahedron triangle)
_midpoint_projection_scale = radius / (2 * g)
sections = ()
for a, b, c in triangles:
ma = _midpoint_projection_scale * (a + b)
mb = _midpoint_projection_scale * (b + c)
mc = _midpoint_projection_scale * (c + a)
sections += (
(ma, mb, mc),
( a, ma, mc),
(ma, b, mb),
(mc, mb, c),
)
del _first_half, _second_half
class _NeighborUmbrella:
index_to_attr = ('x', 'y', 'z')
def __init__(self, components, v0_c0, v1_c1, v2_c1, v4_c4, v0_c4):
self.components = components
self.v0_c0 = self.index_to_attr[v0_c0]
self.v1_c1 = self.index_to_attr[v1_c1]
self.v2_c1 = self.index_to_attr[v2_c1]
self.v4_c4 = self.index_to_attr[v4_c4]
self.v0_c4 = self.index_to_attr[v0_c4]
_neighbor_umbrellas = (
_NeighborUmbrella(( 9, 8, 7, 12, 14), 1, 2, 0, 0, 2),
_NeighborUmbrella(( 1, 2, 4, 5, 3), 0, 0, 2, 2, 0),
_NeighborUmbrella((16, 15, 13, 18, 17), 2, 2, 0, 2, 1),
)
_inverses = (
Matrix3(
Vector3(-0.309017, 0.500000, 0.190983),
Vector3( 0.000000, 0.000000, -0.618034),
Vector3(-0.309017, -0.500000, 0.190983)),
Matrix3(
Vector3(-0.190983, 0.309017, -0.500000),
Vector3(-0.500000, -0.190983, 0.309017),
Vector3( 0.309017, -0.500000, -0.190983)
),
Matrix3(
Vector3(-0.618034, 0.000000, 0.000000),
Vector3( 0.190983, -0.309017, -0.500000),
Vector3( 0.190983, -0.309017, 0.500000)
),
Matrix3(
Vector3(-0.500000, 0.190983, -0.309017),
Vector3( 0.000000, -0.618034, 0.000000),
Vector3( 0.500000, 0.190983, -0.309017)
),
Matrix3(
Vector3(-0.190983, -0.309017, -0.500000),
Vector3(-0.190983, -0.309017, 0.500000),
Vector3( 0.618034, 0.000000, 0.000000)
),
Matrix3(
Vector3(-0.309017, -0.500000, -0.190983),
Vector3( 0.190983, 0.309017, -0.500000),
Vector3( 0.500000, -0.190983, 0.309017)
),
Matrix3(
Vector3( 0.309017, -0.500000, 0.190983),
Vector3( 0.000000, 0.000000, -0.618034),
Vector3( 0.309017, 0.500000, 0.190983)
),
Matrix3(
Vector3( 0.190983, -0.309017, -0.500000),
Vector3( 0.500000, 0.190983, 0.309017),
Vector3(-0.309017, 0.500000, -0.190983)
),
Matrix3(
Vector3( 0.500000, -0.190983, -0.309017),
Vector3( 0.000000, 0.618034, 0.000000),
Vector3(-0.500000, -0.190983, -0.309017)
),
Matrix3(
Vector3( 0.309017, 0.500000, -0.190983),
Vector3(-0.500000, 0.190983, 0.309017),
Vector3(-0.190983, -0.309017, -0.500000)
),
)
_mid_inverses = (
Matrix3(
Vector3(-0.000000, 1.000000, -0.618034),
Vector3( 0.000000, -1.000000, -0.618034),
Vector3(-0.618034, 0.000000, 1.000000)
),
Matrix3(
Vector3(-1.000000, 0.618034, -0.000000),
Vector3(-0.000000, -1.000000, 0.618034),
Vector3( 0.618034, -0.000000, -1.000000)
),
Matrix3(
Vector3(-0.618034, -0.000000, -1.000000),
Vector3( 1.000000, -0.618034, -0.000000),
Vector3(-0.618034, 0.000000, 1.000000)
),
Matrix3(
Vector3(-1.000000, -0.618034, -0.000000),
Vector3( 1.000000, -0.618034, 0.000000),
Vector3(-0.000000, 1.000000, -0.618034)
),
Matrix3(
Vector3(-1.000000, -0.618034, 0.000000),
Vector3( 0.618034, 0.000000, 1.000000),
Vector3( 0.618034, 0.000000, -1.000000)
),
Matrix3(
Vector3(-0.618034, -0.000000, -1.000000),
Vector3( 1.000000, 0.618034, -0.000000),
Vector3( 0.000000, -1.000000, 0.618034)
),
Matrix3(
Vector3( 0.000000, -1.000000, -0.618034),
Vector3( 0.000000, 1.000000, -0.618034),
Vector3( 0.618034, -0.000000, 1.000000)
),
Matrix3(
Vector3( 1.000000, -0.618034, -0.000000),
Vector3( 0.000000, 1.000000, 0.618034),
Vector3(-0.618034, 0.000000, -1.000000)
),
Matrix3(
Vector3( 1.000000, 0.618034, -0.000000),
Vector3(-1.000000, 0.618034, 0.000000),
Vector3( 0.000000, -1.000000, -0.618034)
),
Matrix3(
Vector3(-0.000000, 1.000000, 0.618034),
Vector3(-1.000000, -0.618034, -0.000000),
Vector3( 0.618034, 0.000000, -1.000000)
),
)
def get_section_hit(v):
i = _triangle_index(v)
if i < 0:
return -1
j = _subtriangle_index(i, v)
if j < 0:
return -1
return 4 * i + j
def _neighbor_umbrella_component(idx, comp_idx):
if idx < 3:
return _neighbor_umbrellas[idx].components[comp_idx]
return (_neighbor_umbrellas[idx % 3].components[comp_idx] + 10) % 20
def _from_neighbor_umbrella(idx, v, u):
# TODO: fix equality tests for floating point values (here and in the rest
# of this file)
if u.x == u.y:
comp = _neighbor_umbrella_component(idx, 0)
w = _inverses[comp % 10] * v
if comp > 9:
w = -w
x0 = getattr(w, _neighbor_umbrellas[idx % 3].v0_c0)
if x0 <= 0:
return -1
return comp
if u.y > u.x:
comp = _neighbor_umbrella_component(idx, 1)
w = _inverses[comp % 10] * v
if comp > 9:
w = -w
x1 = getattr(w, _neighbor_umbrellas[idx % 3].v1_c1)
x2 = getattr(w, _neighbor_umbrellas[idx % 3].v2_c1)
if x1 == 0:
return -1
elif x1 < 0:
return _neighbor_umbrella_component(idx, 2)
if x2 == 0:
return -1
elif x2 < 0:
return _neighbor_umbrella_component(idx, 0)
return comp
else:
comp = _neighbor_umbrella_component(idx, 4)
w = _inverses[comp % 10] * v
if comp > 9:
w = -w
x4 = getattr(w, _neighbor_umbrellas[idx % 3].v4_c4)
x0 = getattr(w, _neighbor_umbrellas[idx % 3].v0_c4)
if x4 == 0:
return -1
elif x4 < 0:
return _neighbor_umbrella_component(idx, 0)
if x0 == 0:
return -1
elif x0 < 0:
return _neighbor_umbrella_component(idx, 3)
return comp
def _triangle_index(v):
w = _inverses[0] * v
zero_count = 0
balance = 0
umbrella = -1
if w.x == 0:
zero_count += 1
elif w.x > 0:
balance += 1
else:
balance -= 1
if w.y == 0:
zero_count += 1
elif w.y > 0:
balance += 1
else:
balance -= 1
if w.z == 0:
zero_count += 1
elif w.z > 0:
balance += 1
else:
balance -= 1
if balance == 3:
return 0
elif balance == -3:
return 10
elif balance == 2:
return -1
elif balance == -2:
return -1
elif balance == 1:
if zero_count == 2:
return -1
if w.x < 0:
umbrella = 1
elif w.y < 0:
umbrella = 2
else:
umbrella = 0
elif balance == -1:
if zero_count == 2:
return -1
if w.x > 0:
umbrella = 4
elif w.y > 0:
umbrella = 5
else:
umbrella = 3
w = -w
elif balance == 0:
if zero_count == 3:
return -1
if w.x < 0:
umbrella = 1
elif w.y < 0:
umbrella = 2
else:
umbrella = 0
m = umbrella % 3
if m == 0:
w.z = -w.z
elif m == 1:
w.x, w.y, w.z = w.y, w.z, -w.x
elif m == 2:
w.x, w.y, w.z = w.z, w.x, -w.y
return _from_neighbor_umbrella(umbrella, v, w, inclusive)
def _subtriangle_index(triangle_index, v):
w = _mid_inverses[triangle_index % 10] * v
if triangle_index > 9:
w = -w
if 0 in (w.x, w.y, w.z):
return -1
if w.x < 0:
return 3
if w.y < 0:
return 1
if w.z < 0:
return 2
return 0
MAVProxy-1.8.74/MAVProxy/modules/lib/graph_ui.py 0000664 0000000 0000000 00000006025 15043245150 0021355 0 ustar 00root root 0000000 0000000 from MAVProxy.modules.lib import grapher
from MAVProxy.modules.lib import multiproc
import errno
import socket
import copy
graph_count = 1
class Graph_UI(object):
"""docstring for ClassName"""
def __init__(self, mestate):
self.mestate = mestate
self.xlim = None
global graph_count
self.count = graph_count
graph_count += 1
self.xlim_pipe = multiproc.Pipe()
def display_graph(self, graphdef, flightmode_colourmap=None):
'''display a graph'''
if 'mestate' in globals():
self.mestate.console.write("Expression: %s\n" % ' '.join(graphdef.expression.split()))
else:
self.mestate.child_pipe_send_console.send("Expression: %s\n" % ' '.join(graphdef.expression.split()))
#mestate.mlog.reduce_by_flightmodes(mestate.flightmode_selections)
#setup the graph, then pass to a new process and display
self.mg = grapher.MavGraph(flightmode_colourmap)
if self.mestate.settings.title is not None:
self.mg.set_title(self.mestate.settings.title)
else:
self.mg.set_title(graphdef.name)
if self.mestate.settings.max_rate > 0:
self.mg.set_max_message_rate(self.mestate.settings.max_rate)
self.mg.set_marker(self.mestate.settings.marker)
self.mg.set_condition(self.mestate.settings.condition)
self.mg.set_xaxis(self.mestate.settings.xaxis)
self.mg.set_linestyle(self.mestate.settings.linestyle)
self.mg.set_show_flightmode(self.mestate.settings.show_flightmode)
self.mg.set_legend(self.mestate.settings.legend)
self.mg.add_mav(copy.copy(self.mestate.mlog))
for f in graphdef.expression.split():
self.mg.add_field(f)
self.mg.process(self.mestate.flightmode_selections, self.mestate.mlog._flightmodes)
self.lenmavlist = len(self.mg.mav_list)
#Important - mg.mav_list is the full logfile and can be very large in size
#To avoid slowdowns in Windows (which copies the vars to the new process)
#We need to empty this var when we're finished with it
self.mg.mav_list = []
child = multiproc.Process(target=self.mg.show, args=[self.lenmavlist,], kwargs={"xlim_pipe" : self.xlim_pipe})
child.start()
self.xlim_pipe[1].close()
self.mestate.mlog.rewind()
def check_xlim_change(self):
'''check for new X bounds'''
if self.xlim_pipe is None:
return None
xlim = None
try:
while self.xlim_pipe[0].poll():
xlim = self.xlim_pipe[0].recv()
except Exception:
return None
if xlim != self.xlim:
return xlim
return None
def set_xlim(self, xlim):
'''set new X bounds'''
if self.xlim_pipe is not None and self.xlim != xlim:
#print("send0: ", graph_count, xlim)
try:
self.xlim_pipe[0].send(xlim)
except IOError:
return False
self.xlim = xlim
return True
MAVProxy-1.8.74/MAVProxy/modules/lib/graphdefinition.py 0000664 0000000 0000000 00000000527 15043245150 0022732 0 ustar 00root root 0000000 0000000 '''
GraphDefinition class
'''
class GraphDefinition(object):
'''a pre-defined graph'''
def __init__(self, name, expression, description, expressions, filename):
self.name = name
self.expression = expression
self.description = description
self.expressions = expressions
self.filename = filename
MAVProxy-1.8.74/MAVProxy/modules/lib/grapher.py 0000775 0000000 0000000 00000066422 15043245150 0021221 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
core library for graphing in mavexplorer
'''
import ast
import sys, struct, time, os, datetime, platform
import math, re
import matplotlib
if platform.system() == "Windows":
# wxAgg doesn't properly show the graph values in the lower right on Windows
matplotlib.use('TkAgg')
elif platform.system() != "Darwin" and os.getenv("MPLBACKEND") is None:
# on MacOS we can't set WxAgg here as it conflicts with the MacOS version
matplotlib.use('WXAgg')
from math import *
from pymavlink.mavextra import *
import matplotlib.pyplot as plt
from pymavlink import mavutil
import threading
import numpy as np
MAVGRAPH_DEBUG = 'MAVGRAPH_DEBUG' in os.environ
colors = [ 'red', 'green', 'blue', 'orange', 'olive', 'black', 'grey', 'yellow', 'brown', 'darkcyan',
'cornflowerblue', 'darkmagenta', 'deeppink', 'darkred']
flightmode_colours = [
(1.0, 0, 0),
( 0, 1.0, 0),
( 0, 0, 1.0),
( 0, 1.0, 1.0),
(1.0, 0, 1.0),
(1.0, 1.0, 0),
(1.0, 0.5, 0),
(1.0, 0, 0.5),
(0.5, 1.0, 0),
( 0, 1.0, 0.5),
(0.5, 0, 1.0),
( 0, 0.5, 1.0),
(1.0, 0.5, 0.5),
(0.5, 1.0, 0.5),
(0.5, 0.5, 1.0)
]
edge_colour = (0.1, 0.1, 0.1)
graph_num = 1
tday_base = None
tday_basetime = None
def timestamp_to_days(timestamp, timeshift=0):
'''convert log timestamp to days, public so that mavflightview can access'''
global tday_base, tday_basetime
if tday_base is None:
try:
tday_base = matplotlib.dates.date2num(datetime.datetime.fromtimestamp(timestamp+timeshift))
tday_basetime = timestamp
except ValueError:
# this can happen if the log is corrupt
# ValueError: year is out of range
return 0
sec_to_days = 1.0 / (60*60*24)
return tday_base + (timestamp - tday_basetime) * sec_to_days
class MilliFormatter(matplotlib.dates.AutoDateFormatter):
'''tick formatter that shows millisecond resolution'''
def __init__(self, locator):
super().__init__(locator)
# don't show day until much wider range
self.scaled[1.0/(24*60)] = self.scaled[1.0/(24*60*60)]
def __call__(self, x, pos=0):
"""Return the label for time x at position pos."""
v = super().__call__(x,pos=pos)
if v.endswith("000"):
return v[:-3]
return v
class MavGraph(object):
def __init__(self, flightmode_colourmap=None):
self.lowest_x = None
self.highest_x = None
self.mav_list = []
self.fields = []
self.condition = None
self.xaxis = None
self.marker = None
self.linestyle = None
self.show_flightmode = True
self.legend = 'upper left'
self.legend2 = 'upper right'
self.legend_flightmode = 'lower left'
self.timeshift = 0
self.labels = None
self.multi = False
self.modes_plotted = {}
self.flightmode_colour_index = 0
if flightmode_colourmap:
self.flightmode_colourmap = flightmode_colourmap
else:
self.flightmode_colourmap = {}
self.flightmode_list = None
self.ax1 = None
self.locator = None
global graph_num
self.graph_num = graph_num
self.start_time = None
graph_num += 1
self.draw_events = 0
self.closing = False
self.xlim_pipe = None
self.xlim = None
self.tday_base = None
self.tday_basetime = None
self.title = None
self.grid = False
self.xlim_t = None
if sys.version_info[0] >= 3:
self.text_types = frozenset([str,])
else:
self.text_types = frozenset([unicode, str])
self.max_message_rate = 0
def set_max_message_rate(self, rate_hz):
'''set maximum rate we will graph any message'''
self.max_message_rate = rate_hz
self.last_message_t = {}
def add_field(self, field):
'''add another field to plot'''
self.fields.append(field)
def add_mav(self, mav):
'''add another data source to plot'''
self.mav_list.append(mav)
def set_condition(self, condition):
'''set graph condition'''
self.condition = condition
def set_xaxis(self, xaxis):
'''set graph xaxis'''
self.xaxis = xaxis
def set_title(self, title):
'''set graph title'''
self.title = title
def set_grid(self, enable):
'''enable grid'''
self.grid = enable
def set_marker(self, marker):
'''set graph marker'''
self.marker = marker
def set_timeshift(self, timeshift):
'''set graph timeshift'''
self.timeshift = timeshift
def set_legend2(self, legend2):
'''set graph legend2'''
self.legend2 = legend2
def set_legend(self, legend):
'''set graph legend'''
self.legend = legend
def set_show_flightmode(self, value):
'''set to true if flightmodes are to be shown'''
self.show_flightmode = value
def set_linestyle(self, linestyle):
'''set graph linestyle'''
self.linestyle = linestyle
def set_multi(self, multi):
'''set multiple graph option'''
self.multi = multi
def make_format(self, current, other):
# current and other are axes
def format_coord(x, y):
# x, y are data coordinates
# convert to display coords
display_coord = current.transData.transform((x,y))
inv = other.transData.inverted()
# convert back to data coords with respect to ax
ax_coord = inv.transform(display_coord)
xstr = self.formatter(x)
y2 = ax_coord[1]
if self.xaxis:
return ('x=%.3f Left=%.3f Right=%.3f' % (x, y2, y))
else:
return ('x=%s Left=%.3f Right=%.3f' % (xstr, y2, y))
return format_coord
def next_flightmode_colour(self):
'''allocate a colour to be used for a flight mode'''
if self.flightmode_colour_index > len(flightmode_colours):
print("Out of colours; reusing")
self.flightmode_colour_index = 0
ret = flightmode_colours[self.flightmode_colour_index]
self.flightmode_colour_index += 1
return ret
def flightmode_colour(self, flightmode):
'''return colour to be used for rendering a flight mode background'''
if flightmode not in self.flightmode_colourmap:
self.flightmode_colourmap[flightmode] = self.next_flightmode_colour()
return self.flightmode_colourmap[flightmode]
def xlim_changed(self, axsubplot):
'''called when x limits are changed'''
xrange = axsubplot.get_xbound()
xlim = axsubplot.get_xlim()
if self.draw_events == 0:
# ignore limit change before first draw event
return
if self.xlim_pipe is not None and axsubplot == self.ax1 and xlim != self.xlim:
self.xlim = xlim
#print('send', self.graph_num, xlim)
self.xlim_pipe[1].send(xlim)
def draw_event(self, evt):
'''called on draw events'''
self.draw_events += 1
def close_event(self, evt):
'''called on close events'''
self.closing = True
def rescale_yaxis(self, axis):
'''rescale Y axes to fit'''
ylim = [None,None]
for line in axis.lines:
xdata = line.get_xdata()
xlim = axis.get_xlim()
xidx1 = np.argmax(xdata > xlim[0])
xidx2 = np.argmax(xdata > xlim[1])
if xidx2 == 0:
xidx2 = len(xdata)-1
ydata = line.get_ydata()[xidx1:xidx2]
min_v = np.amin(ydata)
max_v = np.amax(ydata)
if ylim[0] is None or min_v < ylim[0]:
ylim[0] = min_v
if ylim[1] is None or max_v > ylim[1]:
ylim[1] = max_v
rng = ylim[1] - ylim[0]
pad = 0.05 * rng
if pad == 0:
pad = 0.001 * ylim[0]
axis.set_ylim((ylim[0]-pad,ylim[1]+pad))
def button_click(self, event):
'''handle button clicks'''
if getattr(event, 'dblclick', False) and event.button==1:
self.rescale_yaxis(self.ax1)
if self.ax2:
self.rescale_yaxis(self.ax2)
def plotit(self, x, y, fields, colors=[], title=None, interactive=True):
'''plot a set of graphs using date for x axis'''
if interactive:
plt.ion()
self.fig = plt.figure(num=1, figsize=(12,6))
self.ax1 = self.fig.gca()
self.ax2 = None
for i in range(0, len(fields)):
if len(x[i]) == 0: continue
if self.lowest_x is None or x[i][0] < self.lowest_x:
self.lowest_x = x[i][0]
if self.highest_x is None or x[i][-1] > self.highest_x:
self.highest_x = x[i][-1]
if self.highest_x is None or self.lowest_x is None:
return
self.locator = matplotlib.dates.AutoDateLocator()
self.ax1.xaxis.set_major_locator(self.locator)
self.formatter = MilliFormatter(self.locator)
if not self.xaxis:
self.ax1.xaxis.set_major_formatter(self.formatter)
self.ax1.callbacks.connect('xlim_changed', self.xlim_changed)
self.fig.canvas.mpl_connect('draw_event', self.draw_event)
self.fig.canvas.mpl_connect('close_event', self.close_event)
self.fig.canvas.mpl_connect('button_press_event', self.button_click)
self.fig.canvas.get_default_filename = lambda: ''.join("graph" if self.title is None else
(x if x.isalnum() else '_' for x in self.title)) + '.png'
empty = True
ax1_labels = []
ax2_labels = []
for i in range(len(fields)):
if len(x[i]) == 0:
#print("Failed to find any values for field %s" % fields[i])
continue
if i < len(colors):
color = colors[i]
else:
color = 'red'
(tz, tzdst) = time.tzname
if self.axes[i] == 2:
if self.ax2 is None:
self.ax2 = self.ax1.twinx()
if self.grid:
self.ax2.grid(None)
self.ax1.grid(True)
self.ax2.format_coord = self.make_format(self.ax2, self.ax1)
ax = self.ax2
if not self.xaxis:
self.ax2.xaxis.set_major_locator(self.locator)
self.ax2.xaxis.set_major_formatter(self.formatter)
label = fields[i]
if label.endswith(":2"):
label = label[:-2]
ax2_labels.append(label)
if self.custom_labels[i] is not None:
ax2_labels[-1] = self.custom_labels[i]
else:
ax1_labels.append(fields[i])
if self.custom_labels[i] is not None:
ax1_labels[-1] = self.custom_labels[i]
ax = self.ax1
if self.xaxis:
if self.marker is not None:
marker = self.marker
else:
marker = '+'
if self.linestyle is not None:
linestyle = self.linestyle
else:
linestyle = 'None'
ax.plot(x[i], y[i], color=color, label=fields[i],
linestyle=linestyle, marker=marker)
else:
if self.marker is not None:
marker = self.marker
else:
marker = 'None'
if self.linestyle is not None:
linestyle = self.linestyle
else:
linestyle = '-'
if len(y[i]) > 0 and type(y[i][0]) in self.text_types:
# assume this is a piece of text to be rendered at a point in time
last_text_time = -1
last_text = None
for n in range(0, len(x[i])):
this_text_time = round(x[i][n], 6)
this_text = y[i][n]
if last_text is None:
last_text = "[" + this_text + "]"
last_text_time = this_text_time
elif this_text_time == last_text_time:
last_text += ("[" + this_text + "]")
else:
ax.text(last_text_time,
10,
last_text,
rotation=90,
alpha=0.6,
verticalalignment='center')
last_text = this_text
last_text_time = this_text_time
if last_text is not None:
ax.text(last_text_time,
10,
last_text,
rotation=90,
alpha=0.6,
verticalalignment='center')
else:
ax.plot_date(x[i], y[i], fmt=color, label=fields[i],
linestyle=linestyle, marker=marker, tz=None)
empty = False
if self.grid:
plt.grid()
if self.show_flightmode != 0:
alpha = 0.3
xlim = self.ax1.get_xlim()
for i in range(len(self.flightmode_list)):
(mode_name,t0,t1) = self.flightmode_list[i]
c = self.flightmode_colour(mode_name)
tday0 = timestamp_to_days(t0, self.timeshift)
tday1 = timestamp_to_days(t1, self.timeshift)
if tday0 > xlim[1] or tday1 < xlim[0]:
continue
tday0 = max(tday0, xlim[0])
tday1 = min(tday1, xlim[1])
self.ax1.axvspan(tday0, tday1, fc=c, ec=edge_colour, alpha=alpha)
self.modes_plotted[mode_name] = (c, alpha)
if empty:
print("No data to graph")
return
if title is not None:
plt.title(title)
else:
title = fields[0]
if self.fig.canvas.manager is not None:
self.fig.canvas.manager.set_window_title(title)
else:
self.fig.canvas.set_window_title(title)
if self.show_flightmode != 0:
mode_patches = []
for mode in self.modes_plotted.keys():
(color, alpha) = self.modes_plotted[mode]
mode_patches.append(matplotlib.patches.Patch(color=color,
label=mode, alpha=alpha*1.5))
labels = [patch.get_label() for patch in mode_patches]
if ax1_labels != [] and self.show_flightmode != 2:
patches_legend = plt.legend(mode_patches, labels, loc=self.legend_flightmode)
self.fig.gca().add_artist(patches_legend)
else:
plt.legend(mode_patches, labels)
if ax1_labels != []:
self.ax1.legend(ax1_labels,loc=self.legend)
if ax2_labels != []:
self.ax2.legend(ax2_labels,loc=self.legend2)
def add_data(self, t, msg, vars):
'''add some data'''
mtype = msg.get_type()
for i in range(0, len(self.fields)):
if mtype not in self.field_types[i]:
continue
f = self.fields[i]
has_instance = False
ins_value = None
if mtype in self.instance_types[i]:
instance_field = getattr(msg,'instance_field',None)
if instance_field is None and hasattr(msg,'fmt'):
instance_field = getattr(msg.fmt,'instance_field')
if instance_field is not None:
ins_value = getattr(msg,instance_field,None)
if ins_value is None or not str(ins_value) in self.instance_types[i][mtype]:
continue
if not mtype in vars or not isinstance(vars[mtype], dict):
vars[mtype] = dict()
vars[mtype][ins_value] = msg
if isinstance(ins_value, str):
mtype_instance = '%s[%s]' % (mtype, ins_value)
mtype_instance_str = '%s["%s"]' % (mtype, getattr(msg, instance_field))
f = f.replace(mtype_instance, mtype_instance_str)
has_instance = True
# allow for capping the displayed message rate
if self.max_message_rate > 0:
mtype_ins = (mtype,ins_value,f)
mt = msg._timestamp
if mtype_ins in self.last_message_t:
dt = mt - self.last_message_t[mtype_ins]
if dt < 1.0 / self.max_message_rate:
continue
self.last_message_t[mtype_ins] = mt
simple = self.simple_field[i]
v = None
if simple is not None and not has_instance:
try:
v = getattr(vars[simple[0]], simple[1])
except Exception as ex:
if MAVGRAPH_DEBUG:
print(ex)
if v is None:
try:
v = mavutil.evaluate_expression(f, vars)
except Exception as ex:
if MAVGRAPH_DEBUG:
print(ex)
if v is None:
continue
if self.xaxis is None:
xv = t
else:
xv = mavutil.evaluate_expression(self.xaxis, vars)
if xv is None:
continue
self.y[i].append(v)
self.x[i].append(xv)
def process_mav(self, mlog, flightmode_selections):
'''process one file'''
self.vars = {}
idx = 0
all_false = True
for s in flightmode_selections:
if s:
all_false = False
self.num_fields = len(self.fields)
self.custom_labels = [None] * self.num_fields
for i in range(self.num_fields):
if self.fields[i].endswith(">"):
a2 = self.fields[i].rfind("<")
if a2 != -1:
self.custom_labels[i] = self.fields[i][a2+1:-1]
self.fields[i] = self.fields[i][:a2]
# pre-calc right/left axes
for i in range(self.num_fields):
f = self.fields[i]
if f.endswith(":2"):
self.axes[i] = 2
f = f[:-2]
if f.endswith(":1"):
self.first_only[i] = True
f = f[:-2]
self.fields[i] = f
# see which fields are simple
self.simple_field = []
for i in range(0, self.num_fields):
f = self.fields[i]
m = re.match('^([A-Z][A-Z0-9_]*)[.]([A-Za-z_][A-Za-z0-9_]*)$', f)
if m is None:
self.simple_field.append(None)
else:
self.simple_field.append((m.group(1),m.group(2)))
if len(self.flightmode_list) > 0:
# prime the timestamp conversion
timestamp_to_days(self.flightmode_list[0][1], self.timeshift)
try:
reset_state_data()
except Exception:
pass
all_messages = {}
while True:
msg = mlog.recv_match(type=self.msg_types)
if msg is None:
break
mtype = msg.get_type()
if not mtype in all_messages or not isinstance(all_messages[mtype],dict):
all_messages[mtype] = msg
if mtype not in self.msg_types:
continue
if self.condition:
if not mavutil.evaluate_condition(self.condition, all_messages):
continue
tdays = timestamp_to_days(msg._timestamp, self.timeshift)
if all_false or len(flightmode_selections) == 0:
self.add_data(tdays, msg, all_messages)
else:
if idx < len(self.flightmode_list) and msg._timestamp >= self.flightmode_list[idx][2]:
idx += 1
elif (idx < len(flightmode_selections) and flightmode_selections[idx]):
self.add_data(tdays, msg, all_messages)
def xlim_change_check(self, idx):
'''handle xlim change requests from queue'''
try:
if not self.xlim_pipe[1].poll():
return
xlim = self.xlim_pipe[1].recv()
if xlim is None:
return
except Exception:
return
#print("recv: ", self.graph_num, xlim)
if self.ax1 is not None and xlim != self.xlim:
self.xlim = xlim
self.fig.canvas.toolbar.push_current()
#print("setting: ", self.graph_num, xlim)
self.ax1.set_xlim(xlim)
def xlim_timer(self):
'''called every 0.1s to check for xlim change'''
if self.closing:
return
self.xlim_change_check(0)
def process(self, flightmode_selections, _flightmodes, block=True):
'''process and display graph'''
self.msg_types = set()
self.multiplier = []
self.field_types = []
self.instance_types = []
self.xlim = None
self.flightmode_list = _flightmodes
# work out msg types we are interested in
self.x = []
self.y = []
self.modes = []
self.axes = []
self.first_only = []
re_caps = re.compile('[A-Z_][A-Z0-9_]+')
re_instance = re.compile(r'([A-Z_][A-Z0-9_]+)\[([0-9A-Z_]+)\]')
for f in self.fields:
caps = set(re.findall(re_caps, f))
self.msg_types = self.msg_types.union(caps)
self.field_types.append(caps)
instances = set(re.findall(re_instance, f))
itypes = dict()
for (itype,ivalue) in instances:
if not itype in itypes:
itypes[itype] = set()
itypes[itype].add(ivalue)
self.instance_types.append(itypes)
self.y.append([])
self.x.append([])
self.axes.append(1)
self.first_only.append(False)
timeshift = self.timeshift
for fi in range(0, len(self.mav_list)):
mlog = self.mav_list[fi]
self.process_mav(mlog, flightmode_selections)
def show(self, lenmavlist, block=True, xlim_pipe=None, output=None):
'''show graph'''
if self.closing:
return
if xlim_pipe is not None:
xlim_pipe[0].close()
self.xlim_pipe = xlim_pipe
if self.labels is not None:
labels = self.labels.split(',')
if len(labels) != len(fields)*lenmavlist:
print("Number of labels (%u) must match number of fields (%u)" % (
len(labels), len(fields)*lenmavlist))
return
else:
labels = None
for fi in range(0, lenmavlist):
timeshift = 0
for i in range(0, len(self.x)):
if self.first_only[i] and fi != 0:
self.x[i] = []
self.y[i] = []
if labels:
lab = labels[fi*len(self.fields):(fi+1)*len(self.fields)]
else:
lab = self.fields[:]
if self.multi:
col = colors[:]
else:
col = colors[fi*len(self.fields):]
interactive = True
if output is not None:
interactive = False
self.plotit(self.x, self.y, lab, colors=col, title=self.title, interactive=interactive)
for i in range(0, len(self.x)):
self.x[i] = []
self.y[i] = []
if self.xlim_pipe is not None and output is None:
import matplotlib.animation
self.ani = matplotlib.animation.FuncAnimation(self.fig, self.xlim_change_check,
frames=10, interval=20000,
repeat=True, blit=False)
if self.xlim_t is None:
self.xlim_t = self.fig.canvas.new_timer(interval=100)
self.xlim_t.add_callback(self.xlim_timer)
self.xlim_t.start()
if output is None:
plt.draw()
plt.show(block=block)
elif output.endswith(".html"):
import mpld3
html = mpld3.fig_to_html(self.fig)
f_out = open(output, 'w')
f_out.write(html)
f_out.close()
else:
plt.savefig(output, bbox_inches='tight', dpi=200)
if __name__ == "__main__":
from argparse import ArgumentParser
parser = ArgumentParser(description=__doc__)
parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
parser.add_argument("--planner", action='store_true', help="use planner file format")
parser.add_argument("--condition", default=None, help="select packets by a condition")
parser.add_argument("--labels", default=None, help="comma separated field labels")
parser.add_argument("--legend", default='upper left', help="default legend position")
parser.add_argument("--legend2", default='upper right', help="default legend2 position")
parser.add_argument("--marker", default=None, help="point marker")
parser.add_argument("--linestyle", default=None, help="line style")
parser.add_argument("--xaxis", default=None, help="X axis expression")
parser.add_argument("--title", default=None, help="set title")
parser.add_argument("--multi", action='store_true', help="multiple files with same colours")
parser.add_argument("--zero-time-base", action='store_true', help="use Z time base for DF logs")
parser.add_argument("--show-flightmode", default=True,
help="Add background colour to plot corresponding to current flight mode. Cannot be specified with --xaxis.")
parser.add_argument("--dialect", default="all", help="MAVLink dialect")
parser.add_argument("--output", default=None, help="provide an output format")
parser.add_argument("--timeshift", type=float, default=0, help="shift time on first graph in seconds")
parser.add_argument("--grid", action='store_true', help="show a grid")
parser.add_argument("logs_fields", metavar="", nargs="+")
args = parser.parse_args()
mg = MavGraph()
filenames = []
for f in args.logs_fields:
if os.path.exists(f):
mlog = mavutil.mavlink_connection(f, notimestamps=args.notimestamps,
zero_time_base=args.zero_time_base,
dialect=args.dialect)
mg.add_mav(mlog)
else:
mg.add_field(f)
mg.set_condition(args.condition)
mg.set_xaxis(args.xaxis)
mg.set_marker(args.marker)
mg.set_legend(args.legend)
mg.set_legend2(args.legend2)
mg.set_multi(args.multi)
mg.set_title(args.title)
mg.set_grid(args.grid)
mg.set_show_flightmode(args.show_flightmode)
mg.process([],[],0)
mg.show(len(mg.mav_list), output=args.output)
MAVProxy-1.8.74/MAVProxy/modules/lib/icon.py 0000664 0000000 0000000 00000006151 15043245150 0020507 0 ustar 00root root 0000000 0000000 """
MAVProxy icons generator
"""
import numpy as np
from io import BytesIO
from matplotlib.figure import Figure
from matplotlib.gridspec import GridSpec
import matplotlib.path as mpath
import matplotlib.patches as mpatches
from MAVProxy.modules.lib.wx_loader import wx
class SimpleIcon():
"""
Generate icons with Matplotlib.
"""
def __init__(self, text=None):
LIGHT_BLUE_BACKGROUND = "#afceff"
VIOLET_FONT = "#4800f0"
t1 = np.arange(0.0, 5.0, 0.1)
t2 = np.arange(0.0, 5.0, 0.02)
def f(t):
return np.exp(-t) * np.cos(2*np.pi*t)
fig = Figure(figsize=(1.28, 1.28), facecolor=LIGHT_BLUE_BACKGROUND)
spec = GridSpec(nrows=2, ncols=1, wspace=0, hspace=0.0, height_ratios=[1, 2])
ax = fig.add_subplot(spec[0])
ax.text(0, 0.8, 'MAV', style='italic', fontsize=20, weight='bold', color=VIOLET_FONT)
if text is None:
text = "GRAPH"
ax.text(0, 0.1, text.upper(), style='italic', fontsize=14, weight='bold', color=VIOLET_FONT)
ax.axis('off')
ax = fig.add_subplot(spec[1], facecolor=LIGHT_BLUE_BACKGROUND)
if text == "GRAPH" or text == "EXPLORER":
ax.plot(t1, f(t1), color='tab:green', marker='o')
ax.plot(t2, f(t2), color='red')
ax.set_xticks(t1, minor=True)
ax.grid(True, which='both', linestyle="--")
if text == "CONSOLE":
ax.plot([0, 2.5], [2.5, 1.25], color='red', linewidth=4)
ax.plot([0, 2.5], [0, 1.25], color='red', linewidth=4)
ax.plot([3, 5], [0, 0], color='red', linewidth=4)
ax.axis('off')
if text == "MAP":
path_data = [
(mpath.Path.MOVETO, [2.5, 0]),
(mpath.Path.LINETO, [1.25, 2.5]),
(mpath.Path.CURVE3, [2.5, 5]),
(mpath.Path.CURVE3, [3.75, 2.5]),
(mpath.Path.LINETO, [3.75, 2.5]),
(mpath.Path.CLOSEPOLY, [2.5, 0])]
codes, verts = zip(*path_data)
path = mpath.Path(verts, codes)
patch = mpatches.PathPatch(path, facecolor='red', lw=2)
ax.add_patch(patch)
circle_neg = mpatches.Circle((2.5, 2.5), 0.5, color='white')
ax.add_patch(circle_neg)
ax.plot([0, 5/3.0, 10/3.0, 5], [5, 4, 5, 4], color='black', linewidth=2)
ax.plot([0, 5/3.0, 10/3.0, 5], [-1, -2, -1, -2], color='black', linewidth=2)
ax.plot([0, 0], [-1, 5], color='black', linewidth=2)
ax.plot([5/3.0, 5/3.0], [-2, 4], color='black', linewidth=2)
ax.plot([10/3.0, 10/3.0], [-1, 5], color='black', linewidth=2)
ax.plot([5, 5], [-2, 4], color='black', linewidth=2)
ax.set_xlim(-0.5, 5.5)
ax.set_ylim(-2.5, 5.5)
ax.axis('off')
buf = BytesIO()
fig.savefig(buf, format='png')
buf.seek(0)
svgimg = wx.Image(buf, wx.BITMAP_TYPE_PNG)
self.img = svgimg
def get_ico(self):
"""Get the ico from the image bitmap.
need to be call after the wx.app is created."""
return wx.Icon(wx.Bitmap(self.img))
MAVProxy-1.8.74/MAVProxy/modules/lib/kmlread.py 0000664 0000000 0000000 00000014626 15043245150 0021204 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
AP_FLAKE8_CLEAN
'''
import lxml.etree as etree
from io import BytesIO as SIO
from zipfile import ZipFile
import pathlib
import re
namespaces = {
'kml': 'http://www.opengis.net/kml/2.2',
'gx': 'http://www.google.com/kml/ext/2.2',
}
class Style():
def __init__(self, id):
self.id = id
self.line_colour = None
class StyleMap():
def __init__(self, id, otherstyle):
self.id = id
self.otherstyle = otherstyle
class Polygon():
def __init__(self, name, latlon, line_colour=None):
self.name = name
self.vertexes = latlon
self.line_colour = line_colour
class Point():
def __init__(self, name, latlon):
self.name = name
self.latlon = latlon
def readkmz(filename):
'''reads in a kmz file and returns xml nodes'''
xpath = xpath = ".//kml:Placemark"
tree = etree_for_filepath(filename)
return tree.findall(xpath, namespaces)
def etree_for_filepath(filename):
'''reads in a kmz file and returns lxml.etree.ElementTree'''
# Strip quotation marks if neccessary
filename.strip('"')
# Open the zip file (as applicable)
suffix = pathlib.Path(filename).suffix
if suffix.lower() == '.kml':
fo = open(filename, "rb")
fstring = fo.read()
fo.close()
elif suffix.lower() == '.kmz':
zip = ZipFile(filename)
fstring = None
for z in zip.filelist:
if z.filename[-4:] == '.kml':
fstring = zip.read(z)
break
if fstring is None:
raise Exception("Could not find kml file in %s" % filename)
else:
raise Exception(f"load expects a .kml or .kmz file, got ({suffix}) from ({filename})")
parser = etree.XMLParser(encoding='utf-8', recover=True)
return etree.parse(SIO(fstring), parser)
def find_tag(node, tagname):
for c in node.getchildren():
if c.tag == "{" + namespaces['kml'] + "}" + tagname:
return c
return None
def find_tag_recursive(node, tagname):
for c in node.getchildren():
if c.tag == "{" + namespaces['kml'] + "}" + tagname:
return c
if hasattr(c, 'getchildren'):
ret = find_tag_recursive(c, tagname)
if ret is not None:
return ret
return None
def readObject(innode):
'''reads in a node and returns as a tuple: (type, name, points[])'''
# get name
name = find_tag(innode, 'name')
if name is None:
return None
point = find_tag(innode, 'Point')
if point is not None:
coordinates = find_tag(point, 'coordinates')
if coordinates is None:
return None
s = coordinates.text.split(',')
return Point(name.text, (float(s[1]), float(s[0])))
coordinates = find_tag_recursive(innode, 'coordinates')
if coordinates is not None:
latlon = []
for c in coordinates.text.split():
s = c.split(',')
latlon.append((float(s[1]), float(s[0])))
return Polygon(name.text, latlon)
return ('Unknown', None, None)
class KMLRead():
def __init__(self, filepath):
self.filepath = filepath
def placemark_nodes(self):
return self.tree.findall(".//kml:Placemark", namespaces)
def readObject(self, innode):
'''reads in a node and returns as a tuple: (type, name, points[])'''
# get name
name = find_tag(innode, 'name')
if name is None:
return None
point = find_tag(innode, 'Point')
if point is not None:
coordinates = find_tag(point, 'coordinates')
if coordinates is None:
return None
s = coordinates.text.split(',')
return Point(name.text, (float(s[1]), float(s[0])))
coordinates = find_tag_recursive(innode, 'coordinates')
if coordinates is not None:
# a Polygon
latlon = []
for c in coordinates.text.split():
s = c.split(',')
latlon.append((float(s[1]), float(s[0])))
styleURL = find_tag_recursive(innode, 'styleUrl')
line_colour = None
if styleURL is not None:
styleurl_name = styleURL.text.lstrip('#')
if styleurl_name in self.stylemap:
stylemap = self.stylemap[styleurl_name]
otherstyle = stylemap.otherstyle
if otherstyle in self.style:
style = self.style[otherstyle]
line_colour = style.line_colour
return Polygon(name.text, latlon, line_colour=line_colour)
return ('Unknown', None, None)
def parse(self):
self.tree = etree_for_filepath(self.filepath)
# extract styles:
self.style = {}
for s in self.tree.findall(".//gx:CascadingStyle", namespaces):
idname = f"{{{namespaces['kml']}}}id"
_id = s.get(idname)
style = Style(_id)
self.style[_id] = style
line_style = find_tag_recursive(s, 'LineStyle')
if line_style is None:
continue
colour = find_tag_recursive(line_style, 'color')
g = re.match("(?P..)(?P..)(?P..)(?P..)", colour.text)
if g is None:
continue
style.line_colour = (
int(g.group("r"), 16),
int(g.group("g"), 16),
int(g.group("b"), 16)
)
# extract stylemaps:
self.stylemap = {}
for s in self.tree.findall(".//kml:StyleMap", namespaces):
_id = s.get("id")
styleurl_obj = None
for pair in s.getchildren():
if pair is None:
continue
if find_tag_recursive(pair, "key").text == "normal":
styleurl_obj = find_tag_recursive(pair, "styleUrl")
if styleurl_obj is not None:
break
break
if styleurl_obj is None:
continue
stylemap = StyleMap(_id, styleurl_obj.text.lstrip('#'))
self.stylemap[_id] = stylemap
if __name__ == '__main__':
import sys
kml = KMLRead(sys.argv[1])
kml.parse()
for n in kml.placemark_nodes():
obj = kml.readObject(n)
if obj is None:
continue
# for n in readkmz(sys.argv[1]):
# print(readObject(n))
MAVProxy-1.8.74/MAVProxy/modules/lib/live_graph.py 0000775 0000000 0000000 00000006453 15043245150 0021707 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
"""
MAVProxy realtime graphing module, partly based on the wx graphing
demo by Eli Bendersky (eliben@gmail.com)
http://eli.thegreenplace.net/files/prog_code/wx_mpl_dynamic_graph.py.txt
"""
import platform
from MAVProxy.modules.lib import mp_util
from MAVProxy.modules.lib import multiproc
class LiveGraph():
'''
a live graph object using wx and matplotlib
All of the GUI work is done in a child process to provide some insulation
from the parent mavproxy instance and prevent instability in the GCS
New data is sent to the LiveGraph instance via a pipe
'''
def __init__(self,
fields,
title='MAVProxy: LiveGraph',
timespan=20.0,
tickresolution=0.2,
colors=[ 'red', 'green', 'blue', 'orange', 'olive', 'cyan', 'magenta', 'brown',
'violet', 'purple', 'grey', 'black'],
labels=None):
self.fields = fields
self.labels = labels
self.colors = colors
self.title = title
self.timespan = timespan
self.tickresolution = tickresolution
self.values = [None]*len(self.fields)
self.parent_pipe,self.child_pipe = multiproc.Pipe()
self.close_graph = multiproc.Event()
self.close_graph.clear()
self.child = multiproc.Process(target=self.child_task)
self.child.start()
def child_task(self):
'''child process - this holds all the GUI elements'''
mp_util.child_close_fds()
import matplotlib, platform
if platform.system() != "Darwin":
# on MacOS we can't set WxAgg here as it conflicts with the MacOS version
matplotlib.use('WXAgg')
from MAVProxy.modules.lib import wx_processguard
from MAVProxy.modules.lib.wx_loader import wx
app = wx.App(False)
from MAVProxy.modules.lib import live_graph_ui
app.frame = live_graph_ui.GraphFrame(state=self)
app.frame.Show()
app.MainLoop()
def add_values(self, values):
'''add some data to the graph'''
if self.child.is_alive():
self.parent_pipe.send(values)
def close(self):
'''close the graph'''
self.close_graph.set()
if self.is_alive():
self.child.join(2)
def is_alive(self):
'''check if graph is still going'''
return self.child.is_alive()
if __name__ == "__main__":
multiproc.freeze_support()
# test the graph
import time, math
import live_graph
livegraph = live_graph.LiveGraph(['sin(t)', 'cos(t)', 'sin(t+1)',
'cos(t+1)', 'sin(t+2)', 'cos(t+2)',
'cos(t+1)', 'sin(t+2)', 'cos(t+2)', 'x'],
timespan=30,
title='Graph Test',
labels=['a','b','c','d'])
while livegraph.is_alive():
t = time.time()
livegraph.add_values([math.sin(t), math.cos(t),
math.sin(t+1), math.cos(t+1),
math.sin(t+1), math.cos(t+1),
math.sin(t+1), math.cos(t+1),
math.sin(t+2), math.cos(t+2)])
time.sleep(0.05)
MAVProxy-1.8.74/MAVProxy/modules/lib/live_graph_ui.py 0000664 0000000 0000000 00000017130 15043245150 0022373 0 ustar 00root root 0000000 0000000 from MAVProxy.modules.lib.wx_loader import wx
from MAVProxy.modules.lib import icon
import time
import numpy, pylab
class GraphFrame(wx.Frame):
""" The main frame of the application
"""
def __init__(self, state):
wx.Frame.__init__(self, None, -1, state.title)
try:
self.SetIcon(icon.SimpleIcon().get_ico())
except Exception:
pass
self.state = state
self.data = []
for i in range(len(state.fields)):
self.data.append([])
self.paused = False
self.clear_data = False
self.create_main_panel()
self.Bind(wx.EVT_IDLE, self.on_idle)
self.redraw_timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_redraw_timer, self.redraw_timer)
self.redraw_timer.Start(int(1000*self.state.tickresolution))
self.last_yrange = (None, None)
def create_main_panel(self):
import platform
if platform.system() == 'Darwin':
from MAVProxy.modules.lib.MacOS import backend_wxagg
FigCanvas = backend_wxagg.FigureCanvasWxAgg
else:
from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigCanvas
self.panel = wx.Panel(self)
self.init_plot()
self.canvas = FigCanvas(self.panel, -1, self.fig)
self.close_button = wx.Button(self.panel, -1, "Close")
self.Bind(wx.EVT_BUTTON, self.on_close_button, self.close_button)
self.pause_button = wx.Button(self.panel, -1, "Pause")
self.Bind(wx.EVT_BUTTON, self.on_pause_button, self.pause_button)
self.Bind(wx.EVT_UPDATE_UI, self.on_update_pause_button, self.pause_button)
self.clear_button = wx.Button(self.panel, -1, "Clear")
self.Bind(wx.EVT_BUTTON, self.on_clear_button, self.clear_button)
self.Bind(wx.EVT_UPDATE_UI, self.on_update_clear_button, self.clear_button)
did_one = False
self.hbox1 = wx.BoxSizer(wx.HORIZONTAL)
for button in self.close_button, self.pause_button, self.clear_button:
if did_one:
self.hbox1.Add(self.close_button, border=5, flag=wx.ALL | wx.ALIGN_CENTER_VERTICAL)
self.hbox1.AddSpacer(1)
did_one = True
self.hbox1.Add(button, border=5, flag=wx.ALL | wx.ALIGN_CENTER_VERTICAL)
self.vbox = wx.BoxSizer(wx.VERTICAL)
self.vbox.Add(self.canvas, 1, flag=wx.LEFT | wx.TOP | wx.GROW)
self.vbox.Add(self.hbox1, 0, flag=wx.ALIGN_LEFT | wx.TOP)
self.panel.SetSizer(self.vbox)
self.vbox.Fit(self)
def init_plot(self):
self.dpi = 100
from matplotlib.figure import Figure
self.fig = Figure((6.0, 3.0), dpi=self.dpi)
self.axes = self.fig.add_subplot(111)
try:
self.axes.set_facecolor('white')
except AttributeError as e:
# this was removed in matplotlib 2.2.0:
self.axes.set_axis_bgcolor('white')
pylab.setp(self.axes.get_xticklabels(), fontsize=8)
pylab.setp(self.axes.get_yticklabels(), fontsize=8)
# plot the data as a line series, and save the reference
# to the plotted line series
#
self.plot_data = []
if len(self.data[0]) == 0:
max_y = min_y = 0
else:
max_y = min_y = self.data[0][0]
num_labels = 0 if not self.state.labels else len(self.state.labels)
labels = []
for i in range(len(self.data)):
if i < num_labels and self.state.labels[i] is not None:
label = self.state.labels[i]
else:
label = self.state.fields[i]
labels.append(label)
p = self.axes.plot(
self.data[i],
linewidth=1,
color=self.state.colors[i],
label=label
)[0]
self.plot_data.append(p)
if len(self.data[i]) != 0:
min_y = min(min_y, min(self.data[i]))
max_y = max(max_y, max(self.data[i]))
# create X data
self.xdata = numpy.arange(-self.state.timespan, 0, self.state.tickresolution)
self.axes.set_xbound(lower=self.xdata[0], upper=0)
if min_y == max_y:
self.axes.set_ybound(min_y, max_y+0.1)
self.axes.legend(labels, loc='upper left', bbox_to_anchor=(0, 1.1))
def draw_plot(self):
""" Redraws the plot
"""
state = self.state
if len(self.data[0]) == 0:
print("no data to plot")
return
vhigh = max(self.data[0])
vlow = min(self.data[0])
for i in range(1,len(self.plot_data)):
vhigh = max(vhigh, max(self.data[i]))
vlow = min(vlow, min(self.data[i]))
ymin = vlow - 0.05*(vhigh-vlow)
ymax = vhigh + 0.05*(vhigh-vlow)
if ymin == ymax:
ymax = ymin + 0.1 * ymin
ymin = ymin - 0.1 * ymin
if (ymin, ymax) != self.last_yrange:
self.last_yrange = (ymin, ymax)
if ymax == ymin:
ymin = ymin-0.5
ymax = ymin+1
self.axes.set_ybound(lower=ymin, upper=ymax)
#self.axes.ticklabel_format(useOffset=False, style='plain')
self.axes.grid(True, color='gray')
pylab.setp(self.axes.get_xticklabels(), visible=True)
pylab.setp(self.axes.get_legend().get_texts(), fontsize='small')
for i in range(len(self.plot_data)):
ydata = numpy.array(self.data[i])
xdata = self.xdata
if len(ydata) < len(self.xdata):
xdata = xdata[-len(ydata):]
self.plot_data[i].set_xdata(xdata)
self.plot_data[i].set_ydata(ydata)
self.canvas.draw()
self.canvas.Refresh()
def on_pause_button(self, event):
self.paused = not self.paused
def on_update_pause_button(self, event):
label = "Resume" if self.paused else "Pause"
self.pause_button.SetLabel(label)
def on_update_clear_button(self, event):
pass
def on_clear_button(self, event):
self.clear_data = True
def on_close_button(self, event):
self.redraw_timer.Stop()
self.Destroy()
def on_idle(self, event):
time.sleep(self.state.tickresolution*0.5)
def on_redraw_timer(self, event):
# if paused do not add data, but still redraw the plot
# (to respond to scale modifications, grid change, etc.)
#
state = self.state
if state.close_graph.wait(0.001):
self.redraw_timer.Stop()
self.Destroy()
return
while state.child_pipe.poll():
state.values = state.child_pipe.recv()
if self.paused:
return
if self.clear_data:
self.clear_data = False
for i in range(len(self.plot_data)):
if state.values[i] is not None:
while len(self.data[i]):
self.data[i].pop(0)
for i in range(len(self.plot_data)):
if (type(state.values[i]) == list):
print("ERROR: Cannot plot array of length %d. Use 'graph %s[index]' instead"%(len(state.values[i]), state.fields[i]))
self.redraw_timer.Stop()
self.Destroy()
return
if state.values[i] is not None:
self.data[i].append(state.values[i])
while len(self.data[i]) > len(self.xdata):
self.data[i].pop(0)
for i in range(len(self.plot_data)):
if state.values[i] is None or len(self.data[i]) < 2:
return
self.draw_plot()
MAVProxy-1.8.74/MAVProxy/modules/lib/magfit.py 0000664 0000000 0000000 00000063053 15043245150 0021032 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
fit best estimate of magnetometer offsets, diagonals, off-diagonals, cmot and scaling using WMM target
'''
from MAVProxy.modules.lib import wx_processguard
from MAVProxy.modules.lib.wx_loader import wx
import sys, time, os, math, copy, platform
from pymavlink import mavutil
from pymavlink import mavextra
from pymavlink.rotmat import Vector3
from pymavlink.rotmat import Matrix3
from pymavlink.rotmat import rotations
from MAVProxy.modules.lib import grapher
from MAVProxy.modules.lib.multiproc_util import MPDataLogChildTask
import matplotlib
if platform.system() == "Windows":
matplotlib.use('wxagg')
import matplotlib.pyplot as pyplot
import numpy
import datetime
earth_field = None
declination = None
margs = None
mag_idx = ''
class Correction:
def __init__(self):
self.offsets = Vector3(0.0, 0.0, 0.0)
self.diag = Vector3(1.0, 1.0, 1.0)
self.offdiag = Vector3(0.0, 0.0, 0.0)
self.cmot = Vector3(0.0, 0.0, 0.0)
self.scaling = 1.0
def show_parms(self):
print("COMPASS_OFS%s_X %d" % (mag_idx, int(self.offsets.x)))
print("COMPASS_OFS%s_Y %d" % (mag_idx, int(self.offsets.y)))
print("COMPASS_OFS%s_Z %d" % (mag_idx, int(self.offsets.z)))
print("COMPASS_DIA%s_X %.3f" % (mag_idx, self.diag.x))
print("COMPASS_DIA%s_Y %.3f" % (mag_idx, self.diag.y))
print("COMPASS_DIA%s_Z %.3f" % (mag_idx, self.diag.z))
print("COMPASS_ODI%s_X %.3f" % (mag_idx, self.offdiag.x))
print("COMPASS_ODI%s_Y %.3f" % (mag_idx, self.offdiag.y))
print("COMPASS_ODI%s_Z %.3f" % (mag_idx, self.offdiag.z))
print("COMPASS_MOT%s_X %.3f" % (mag_idx, self.cmot.x))
print("COMPASS_MOT%s_Y %.3f" % (mag_idx, self.cmot.y))
print("COMPASS_MOT%s_Z %.3f" % (mag_idx, self.cmot.z))
print("COMPASS_SCALE%s %.2f" % (mag_idx, self.scaling))
if margs['CMOT']:
print("COMPASS_MOTCT 2")
def RotationIDToString(id):
'''map rotation number to a string'''
return rotations[id].name
def StringToRotationID(s):
'''map rotation string to an index'''
for i in range(len(rotations)):
if rotations[i].name == s:
return i
return 0
def correct(MAG, BAT, c):
'''correct a mag sample, returning a Vector3'''
mag = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
# add the given offsets
mag += c.offsets
# multiply by scale factor
mag *= c.scaling
# apply elliptical corrections
mat = Matrix3(
Vector3(c.diag.x, c.offdiag.x, c.offdiag.y),
Vector3(c.offdiag.x, c.diag.y, c.offdiag.z),
Vector3(c.offdiag.y, c.offdiag.z, c.diag.z))
mag = mat * mag
# apply compassmot corrections
if BAT is not None and hasattr(BAT, 'Curr') and not math.isnan(BAT.Curr):
mag += c.cmot * BAT.Curr
return mag
def get_yaw(ATT,MAG,BAT,c):
'''calculate heading from raw magnetometer and new offsets'''
mag = correct(MAG, BAT, c)
# go via a DCM matrix to match the APM calculation
dcm_matrix = mavextra.rotation_df(ATT)
cos_pitch_sq = 1.0-(dcm_matrix.c.x*dcm_matrix.c.x)
headY = mag.y * dcm_matrix.c.z - mag.z * dcm_matrix.c.y
headX = mag.x * cos_pitch_sq - dcm_matrix.c.x * (mag.y * dcm_matrix.c.y + mag.z * dcm_matrix.c.z)
global declination
yaw = math.degrees(math.atan2(-headY,headX)) + declination
if yaw < 0:
yaw += 360
return yaw
def expected_field(ATT, yaw):
'''return expected magnetic field for attitude'''
global earth_field
roll = ATT.Roll
pitch = ATT.Pitch
rot = Matrix3()
rot.from_euler(math.radians(roll), math.radians(pitch), math.radians(yaw))
field = rot.transposed() * earth_field
return field
data = None
old_corrections = Correction()
def wmm_error(p):
'''world magnetic model error with correction fit'''
p = list(p)
c = copy.copy(old_corrections)
c.offsets = Vector3(p.pop(0), p.pop(0), p.pop(0))
c.scaling = p.pop(0)
if margs['Elliptical']:
c.diag = Vector3(p.pop(0), p.pop(0), p.pop(0))
c.offdiag = Vector3(p.pop(0), p.pop(0), p.pop(0))
else:
c.diag = Vector3(1.0, 1.0, 1.0)
c.offdiag = Vector3(0.0, 0.0, 0.0)
if margs['CMOT']:
c.cmot = Vector3(p.pop(0), p.pop(0), p.pop(0))
ret = 0
for (MAG,ATT,BAT) in data:
yaw = get_yaw(ATT,MAG,BAT,c)
expected = expected_field(ATT, yaw)
observed = correct(MAG,BAT,c)
error = (expected - observed).length()
ret += error
ret /= len(data)
return ret
def fit_WWW():
from scipy import optimize
c = copy.copy(old_corrections)
p = [c.offsets.x, c.offsets.y, c.offsets.z, c.scaling]
if margs['Elliptical']:
p.extend([c.diag.x, c.diag.y, c.diag.z, c.offdiag.x, c.offdiag.y, c.offdiag.z])
if margs['CMOT']:
p.extend([c.cmot.x, c.cmot.y, c.cmot.z])
ofs = margs['OffsetMax']
min_scale = margs['ScaleMin']
max_scale = margs['ScaleMax']
min_scale_delta = 0.00001
bounds = [(-ofs,ofs),(-ofs,ofs),(-ofs,ofs),(min_scale,max(min_scale+min_scale_delta,max_scale))]
if margs['CMOT NoChange']:
bounds[0] = (c.offsets.x, c.offsets.x)
bounds[1] = (c.offsets.y, c.offsets.y)
bounds[2] = (c.offsets.z, c.offsets.z)
if margs['Elliptical']:
min_diag = margs['DiagonalMin']
max_diag = margs['DiagonalMax']
min_offdiag = margs['OffDiagMin']
max_offdiag = margs['OffDiagMax']
for i in range(3):
bounds.append((min_diag,max_diag))
for i in range(3):
bounds.append((min_offdiag,max_offdiag))
if margs['CMOT']:
if margs['CMOT NoChange']:
bounds.append((c.cmot.x, c.cmot.x))
bounds.append((c.cmot.y, c.cmot.y))
bounds.append((c.cmot.z, c.cmot.z))
else:
max_cmot = margs['CMOT Max']
for i in range(3):
bounds.append((-max_cmot,max_cmot))
(p,err,iterations,imode,smode) = optimize.fmin_slsqp(wmm_error, p, bounds=bounds, full_output=True)
if imode != 0:
print("Fit failed: %s" % smode)
sys.exit(1)
p = list(p)
c.offsets = Vector3(p.pop(0), p.pop(0), p.pop(0))
c.scaling = p.pop(0)
if margs['Elliptical']:
c.diag = Vector3(p.pop(0), p.pop(0), p.pop(0))
c.offdiag = Vector3(p.pop(0), p.pop(0), p.pop(0))
else:
c.diag = Vector3(1.0, 1.0, 1.0)
c.offdiag = Vector3(0.0, 0.0, 0.0)
if margs['CMOT']:
c.cmot = Vector3(p.pop(0), p.pop(0), p.pop(0))
else:
c.cmot = Vector3(0.0, 0.0, 0.0)
return c
def remove_offsets(MAG, BAT, c):
'''remove all corrections to get raw sensor data'''
correction_matrix = Matrix3(Vector3(c.diag.x, c.offdiag.x, c.offdiag.y),
Vector3(c.offdiag.x, c.diag.y, c.offdiag.z),
Vector3(c.offdiag.y, c.offdiag.z, c.diag.z))
try:
correction_matrix = correction_matrix.invert()
except Exception:
return None
field = Vector3(MAG.MagX, MAG.MagY, MAG.MagZ)
if BAT is not None and hasattr(BAT,'Curr') and not math.isnan(BAT.Curr):
field -= c.cmot * BAT.Curr
field = correction_matrix * field
field *= 1.0 / c.scaling
field -= Vector3(MAG.OfsX, MAG.OfsY, MAG.OfsZ)
if math.isnan(field.x) or math.isnan(field.y) or math.isnan(field.z):
return None
MAG.MagX = int(field.x)
MAG.MagY = int(field.y)
MAG.MagZ = int(field.z)
return MAG
def magfit(mlog, timestamp_in_range):
'''find best magnetometer offset fit to a log file'''
global earth_field, declination
global data
data = []
ATT = None
BAT = None
mag_msg = margs['Magnetometer']
global mag_idx
if mag_msg[-1].isdigit():
mag_instance = None
mag_idx = mag_msg[-1]
elif mag_msg.endswith('[0]'):
mag_instance = 0
mag_idx = ''
mag_msg = 'MAG'
elif mag_msg.endswith(']'):
mag_instance = int(mag_msg[-2])
mag_idx = str(mag_instance+1)
mag_msg = 'MAG'
else:
mag_instance = None
mag_idx = ''
count = 0
parameters = {}
# get parameters
mlog.rewind()
while True:
msg = mlog.recv_match(type=['PARM'])
if msg is None:
break
parameters[msg.Name] = msg.Value
mlog.rewind()
lat = margs['Lattitude']
lon = margs['Longitude']
if lat != 0 and lon != 0:
earth_field = mavextra.expected_earth_field_lat_lon(lat, lon)
(declination,inclination,intensity) = mavextra.get_mag_field_ef(lat, lon)
print("Earth field: %s strength %.0f declination %.1f degrees" % (earth_field, earth_field.length(), declination))
ATT_NAME = margs['Attitude']
mtypes = ['GPS',mag_msg,ATT_NAME,'BAT']
if ATT_NAME == "XKY0":
mtypes.append('ATT')
last_ATT = None
print("Attitude source %s mtypes=%s" % (ATT_NAME, mtypes))
# extract MAG data
while True:
msg = mlog.recv_match(type=mtypes)
if msg is None:
break
in_range = timestamp_in_range(msg._timestamp)
if in_range < 0:
continue
if in_range > 0:
break
if msg.get_type() == 'GPS' and msg.Status >= 3 and earth_field is None:
earth_field = mavextra.expected_earth_field(msg)
(declination,inclination,intensity) = mavextra.get_mag_field_ef(msg.Lat, msg.Lng)
print("Earth field: %s strength %.0f declination %.1f degrees" % (earth_field, earth_field.length(), declination))
if msg.get_type() == 'ATT':
# needed for XKY0 for yaw
last_ATT = msg
if msg.get_type() == ATT_NAME:
if getattr(msg, 'C', 0) != 0:
# use core zero for EKF attitude
continue
if ATT_NAME == 'XKY0':
if last_ATT is None:
continue
# get yaw from GSF, and roll/pitch from ATT
ATT = last_ATT
ATT.Yaw = math.degrees(msg.YC)
else:
ATT = msg
ATT.Roll += math.degrees(parameters['AHRS_TRIM_X'])
ATT.Pitch += math.degrees(parameters['AHRS_TRIM_Y'])
ATT.Yaw += math.degrees(parameters['AHRS_TRIM_Z'])
if msg.get_type() == 'BAT':
if hasattr(msg,'Instance'):
if margs['BatteryNum'] != msg.Instance+1:
continue
BAT = msg
if msg.get_type() == mag_msg and ATT is not None:
if mag_instance is not None:
if getattr(msg,'I',0) != mag_instance:
continue
if count % margs['Reduce'] == 0:
data.append((msg,ATT,BAT))
count += 1
old_corrections.offsets = Vector3(parameters.get('COMPASS_OFS%s_X' % mag_idx,0.0),
parameters.get('COMPASS_OFS%s_Y' % mag_idx,0.0),
parameters.get('COMPASS_OFS%s_Z' % mag_idx,0.0))
old_corrections.diag = Vector3(parameters.get('COMPASS_DIA%s_X' % mag_idx,1.0),
parameters.get('COMPASS_DIA%s_Y' % mag_idx,1.0),
parameters.get('COMPASS_DIA%s_Z' % mag_idx,1.0))
if old_corrections.diag == Vector3(0,0,0):
old_corrections.diag = Vector3(1,1,1)
old_corrections.offdiag = Vector3(parameters.get('COMPASS_ODI%s_X' % mag_idx,0.0),
parameters.get('COMPASS_ODI%s_Y' % mag_idx,0.0),
parameters.get('COMPASS_ODI%s_Z' % mag_idx,0.0))
if parameters.get('COMPASS_MOTCT',0) == 2:
# only support current based corrections for now
old_corrections.cmot = Vector3(parameters.get('COMPASS_MOT%s_X' % mag_idx,0.0),
parameters.get('COMPASS_MOT%s_Y' % mag_idx,0.0),
parameters.get('COMPASS_MOT%s_Z' % mag_idx,0.0))
old_corrections.scaling = parameters.get('COMPASS_SCALE%s' % mag_idx, None)
if old_corrections.scaling is None or old_corrections.scaling < 0.1:
force_scale = False
old_corrections.scaling = 1.0
else:
force_scale = True
rot = None
orig_orient = int(parameters.get('COMPASS_ORIENT'+mag_idx,0))
new_orient = StringToRotationID(margs['Orientation'])
if orig_orient != new_orient:
rot = rotations[orig_orient].rt * rotations[new_orient].r
# remove existing corrections
data2 = []
for (MAG,ATT,BAT) in data:
MAG = remove_offsets(MAG, BAT, old_corrections)
if MAG is None:
continue
if rot is not None:
v = Vector3(MAG.MagX,MAG.MagY,MAG.MagZ)
v = rot * v
MAG.MagX = v.x
MAG.MagY = v.y
MAG.MagZ = v.z
data2.append((MAG,ATT,BAT))
data = data2
print("Extracted %u points" % len(data))
print("Current: %s diag: %s offdiag: %s cmot: %s scale: %.2f" % (
old_corrections.offsets, old_corrections.diag, old_corrections.offdiag, old_corrections.cmot, old_corrections.scaling))
if len(data) == 0:
return
# do fit
c = fit_WWW()
# normalise diagonals to scale factor
if force_scale:
avgdiag = (c.diag.x + c.diag.y + c.diag.z)/3.0
calc_scale = c.scaling
c.scaling *= avgdiag
min_scale = margs['ScaleMin']
max_scale = margs['ScaleMax']
if c.scaling > max_scale:
c.scaling = max_scale
if c.scaling < min_scale:
c.scaling = min_scale
scale_change = c.scaling / calc_scale
c.diag *= 1.0/scale_change
c.offdiag *= 1.0/scale_change
print("New: %s diag: %s offdiag: %s cmot: %s scale: %.2f" % (
c.offsets, c.diag, c.offdiag, c.cmot, c.scaling))
x = []
corrected = {}
corrected['Yaw'] = []
expected1 = {}
expected2 = {}
uncorrected = {}
uncorrected['Yaw'] = []
yaw_change1 = []
yaw_change2 = []
for i in range(len(data)):
(MAG,ATT,BAT) = data[i]
yaw1 = get_yaw(ATT,MAG,BAT,c)
corrected['Yaw'].append(yaw1)
ef1 = expected_field(ATT, yaw1)
cf = correct(MAG, BAT, c)
yaw2 = get_yaw(ATT,MAG,BAT,old_corrections)
ef2 = expected_field(ATT, yaw2)
uncorrected['Yaw'].append(yaw2)
uf = correct(MAG, BAT, old_corrections)
yaw_change1.append(mavextra.wrap_180(yaw1 - yaw2))
yaw_change2.append(mavextra.wrap_180(yaw1 - ATT.Yaw))
for axis in ['x','y','z']:
if not axis in corrected:
corrected[axis] = []
uncorrected[axis] = []
expected1[axis] = []
expected2[axis] = []
corrected[axis].append(getattr(cf, axis))
uncorrected[axis].append(getattr(uf, axis))
expected1[axis].append(getattr(ef1, axis))
expected2[axis].append(getattr(ef2, axis))
x.append(i)
c.show_parms()
fig, axs = pyplot.subplots(3, 1, sharex=True)
for axis in ['x','y','z']:
axs[0].plot(numpy.array(x), numpy.array(uncorrected[axis]), label='Uncorrected %s' % axis.upper() )
axs[0].plot(numpy.array(x), numpy.array(expected2[axis]), label='Expected %s' % axis.upper() )
axs[0].legend(loc='upper left')
axs[0].set_title('Original')
axs[0].set_ylabel('Field (mGauss)')
axs[1].plot(numpy.array(x), numpy.array(corrected[axis]), label='Corrected %s' % axis.upper() )
axs[1].plot(numpy.array(x), numpy.array(expected1[axis]), label='Expected %s' % axis.upper() )
axs[1].legend(loc='upper left')
axs[1].set_title('Corrected')
axs[1].set_ylabel('Field (mGauss)')
# show change in yaw estimate from old corrections to new
axs[2].plot(numpy.array(x), numpy.array(yaw_change1), label='Mag Yaw Change')
axs[2].plot(numpy.array(x), numpy.array(yaw_change2), label='ATT Yaw Change')
axs[2].set_title('Yaw Change (degrees)')
axs[2].legend(loc='upper left')
pyplot.show(block=False)
class MagFit(MPDataLogChildTask):
'''A class used to launch the MagFitUI in a child process'''
def __init__(self, *args, **kwargs):
'''
Parameters
----------
title : str
The title of the application
mlog : DFReader
A dataflash or telemetry log
xlimits: MAVExplorer.XLimits
An object capturing timestamp limits
'''
super(MagFit, self).__init__(*args, **kwargs)
# all attributes are implicitly passed to the child process
self.title = kwargs['title']
self.xlimits = kwargs['xlimits']
# @override
def child_task(self):
'''Launch the MagFitUI'''
from MAVProxy.modules.lib import wx_processguard
from MAVProxy.modules.lib.wx_loader import wx
# create wx application
app = wx.App(False)
app.frame = MagFitUI(title=self.title,
close_event=self.close_event,
mlog=self.mlog,
timestamp_in_range=self.xlimits.timestamp_in_range)
app.frame.SetDoubleBuffered(True)
app.frame.Show()
app.MainLoop()
class MagFitUI(wx.Dialog):
def __init__(self, title, close_event, mlog, timestamp_in_range):
super(MagFitUI, self).__init__(None, title=title, size=(600, 900), style=wx.DEFAULT_DIALOG_STYLE|wx.RESIZE_BORDER)
# capture the close event, log and timestamp range function
self.close_event = close_event
self.mlog = mlog
self.timestamp_in_range = timestamp_in_range
# events
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.OnTimer, self.timer)
self.timer.Start(100)
self.Bind(wx.EVT_IDLE, self.OnIdle)
# initialise the panels etc.
self.init_ui()
def OnIdle(self, event):
time.sleep(0.05)
def OnTimer(self, event):
'''Periodically check if the close event has been received'''
if self.close_event.wait(0.001):
self.timer.Stop()
self.Destroy()
return
def have_msg(self, msg):
'''see if we have a given message name in the log'''
mid = self.mlog.name_to_id.get(msg,-1)
if mid == -1:
return False
return self.mlog.counts[mid] > 0
def init_ui(self):
'''Initalise the UI elements'''
if not hasattr(self.mlog, 'formats'):
print("Must be DF log")
return
self.panel = wx.Panel(self)
self.vbox = wx.BoxSizer(wx.VERTICAL)
self.vbox.AddStretchSpacer()
self.panel.SetSizer(self.vbox)
self.idmap = {}
self.id_by_string = {}
self.values = {}
self.controls = {}
self.callbacks = {}
self.row = None
msg_names = self.mlog.name_to_id.keys()
mag_format = self.mlog.formats[self.mlog.name_to_id['MAG']]
if 'I' in mag_format.columns:
mag_choices = ['MAG[0]', 'MAG[1]', 'MAG[2]']
else:
mag_choices = ['MAG', 'MAG2', 'MAG3']
att_choices = ['ATT']
if self.have_msg('NKF1'):
att_choices.append('NKF1')
if self.have_msg('XKF1'):
att_choices.append('XKF1')
if self.have_msg('XKY0'):
att_choices.append('XKY0')
if self.have_msg('DCM'):
att_choices.append('DCM')
orientation_choices = [ r.name for r in rotations ]
default_orientation = RotationIDToString(int(self.mlog.params.get("COMPASS_ORIENT", 0)))
# first row, Mag and attitude source
self.StartRow('Source Selection')
self.AddCombo('Magnetometer', mag_choices, callback=self.change_mag)
self.AddCombo('Attitude', att_choices)
self.StartRow('Orientation Selection')
self.AddCombo('Orientation', orientation_choices, default=default_orientation)
self.StartRow('Position')
self.AddSpinFloat("Lattitude", -90, 90, 0.000001, 0, digits=8)
self.AddSpinFloat("Longitude", -180, 180, 0.000001, 0, digits=8)
self.StartRow()
self.AddSpinInteger("Reduce", 1, 20, 1)
self.StartRow('Offset Estimation')
self.AddCheckBox("Offsets", default=True)
self.StartRow()
self.AddSpinInteger("OffsetMax", 500, 3000, 1500)
self.StartRow('Scale Factor Estimation')
self.AddSpinFloat("ScaleMin", 0.25, 4.0, 0.01, 1.0)
self.AddSpinFloat("ScaleMax", 0.25, 4.0, 0.01, 1.0)
self.StartRow('Elliptical Estimation')
self.AddCheckBox("Elliptical")
self.StartRow()
self.AddSpinFloat("DiagonalMin", 0.8, 1.0, 0.01, 0.8)
self.AddSpinFloat("DiagonalMax", 1.0, 1.2, 0.01, 1.2)
self.StartRow()
self.AddSpinFloat("OffDiagMin", -0.2, 0.0, 0.01, -0.2)
self.AddSpinFloat("OffDiagMax", 0, 0.2, 0.01, 0.2)
self.StartRow('Motor Interference Estimation')
self.AddCheckBox("CMOT")
self.AddCheckBox("CMOT NoChange")
self.StartRow()
self.AddSpinInteger("BatteryNum", 1, 8, 1)
self.AddSpinFloat("CMOT Max", 1.0, 100, 0.1, 10)
self.StartRow("Processing")
self.AddButton('Run', callback=self.run)
self.AddButton('Close', callback=self.close)
self.EndRow()
self.Center()
def original_orient(self, idx):
'''get original parameter orientation of a compass'''
mag_idx = '' if idx==0 else str(idx+1)
return int(self.mlog.params.get('COMPASS_ORIENT'+mag_idx,0))
def change_mag(self, cid):
'''change selected mag, update orientation'''
mag = int(self.values['Magnetometer'][4])
orig_orient = self.original_orient(mag)
orient_str = RotationIDToString(orig_orient)
c = self.controls[cid]
orient_id = self.id_by_string['Orientation']
orient_c = self.controls[orient_id]
orient_c.SetValue(orient_str)
self.values['Orientation'] = orient_str
def close(self, cid):
'''Set the close event'''
self.close_event.set()
def StartRow(self, label=None):
if self.row:
self.EndRow()
if label:
self.row = wx.BoxSizer(wx.HORIZONTAL)
text = wx.StaticText(self.panel, label=label)
font = wx.Font(16, wx.DEFAULT, wx.NORMAL, wx.BOLD)
text.SetFont(font)
self.row.Add(text, 0, wx.LEFT, 10)
self.vbox.Add(self.row, 0, wx.TOP, 10)
self.row = wx.BoxSizer(wx.HORIZONTAL)
def EndRow(self):
self.vbox.Add(self.row, 0, wx.TOP, 10)
self.row = None
def AddControl(self, c, label, default):
self.idmap[c.GetId()] = label
self.id_by_string[label] = c.GetId()
self.controls[c.GetId()] = c
self.values[label] = default
def AddCombo(self, label, choices, default=None, callback=None):
if default is None:
default = choices[0]
c = wx.ComboBox(choices=choices,
parent=self.panel,
style=0,
value=default)
self.AddControl(c, label, default)
c.Bind(wx.EVT_COMBOBOX, self.OnValue)
self.row.Add(wx.StaticText(self.panel, label=label), 0, wx.LEFT, 20)
self.row.Add(c, 0, wx.LEFT, 20)
if callback is not None:
self.callbacks[c.GetId()] = callback
def AddButton(self, label, callback=None):
c = wx.Button(self.panel, label=label)
self.AddControl(c, label, False)
if callback is not None:
self.callbacks[c.GetId()] = callback
c.Bind(wx.EVT_BUTTON, self.OnButton)
self.row.Add(c, 0, wx.LEFT, 20)
def AddCheckBox(self, label, default=False):
c = wx.CheckBox(self.panel, label=label)
c.SetValue(default)
self.AddControl(c, label, default)
c.Bind(wx.EVT_CHECKBOX, self.OnValue)
self.row.Add(c, 0, wx.LEFT, 20)
def AddSpinInteger(self, label, min_value, max_value, default):
c = wx.SpinCtrl(self.panel, -1, min=min_value, max=max_value)
c.SetRange(min_value, max_value)
c.SetValue(default)
self.AddControl(c, label, default)
self.row.Add(wx.StaticText(self.panel, label=label), 0, wx.LEFT, 20)
self.row.Add(c, 0, wx.LEFT, 20)
self.row.Add(wx.StaticText(self.panel, label=""), 0, wx.LEFT, 20)
self.Bind(wx.EVT_SPINCTRL, self.OnValue)
def AddSpinFloat(self, label, min_value, max_value, increment, default, digits=4):
c = wx.SpinCtrlDouble(self.panel, -1, min=min_value, max=max_value)
c.SetRange(min_value, max_value)
c.SetValue(default)
c.SetIncrement(increment)
c.SetDigits(digits)
s1 = "%.*f" % (digits, min_value)
s2 = "%.*f" % (digits, max_value)
if len(s1) > len(s2):
s = s1
else:
s = s2
if hasattr(c, 'GetSizeFromText'):
size = c.GetSizeFromText(s+"xx")
c.SetMinSize(size)
self.AddControl(c, label, default)
self.row.Add(wx.StaticText(self.panel, label=label), 0, wx.LEFT, 20)
self.row.Add(c, 0, wx.LEFT, 20)
self.row.Add(wx.StaticText(self.panel, label=""), 0, wx.LEFT, 20)
self.Bind(wx.EVT_SPINCTRLDOUBLE, self.OnValue)
def OnValue(self, event):
self.values[self.idmap[event.GetId()]] = self.controls[event.GetId()].GetValue()
if event.GetId() in self.callbacks:
self.callbacks[event.GetId()](event.GetId())
def OnButton(self, event):
self.values[self.idmap[event.GetId()]] = True
if event.GetId() in self.callbacks:
self.callbacks[event.GetId()](event.GetId())
def run(self, cid):
global margs
margs = self.values
magfit(self.mlog,self.timestamp_in_range)
MAVProxy-1.8.74/MAVProxy/modules/lib/mav_fft.py 0000664 0000000 0000000 00000011347 15043245150 0021204 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
extract ISBH and ISBD messages from AP_Logging files and produce FFT plots
'''
import numpy
import os
import pylab
import sys
import time
from pymavlink import mavutil
from MAVProxy.modules.lib.multiproc_util import MPDataLogChildTask
class MavFFT(MPDataLogChildTask):
'''A class used to launch `mavfft_display` in a child process'''
def __init__(self, *args, **kwargs):
'''
Parameters
----------
mlog : DFReader
A dataflash or telemetry log
xlimits: MAVExplorer.XLimits
An object capturing timestamp limits
'''
super(MavFFT, self).__init__(*args, **kwargs)
# all attributes are implicitly passed to the child process
self.xlimits = kwargs['xlimits']
# @override
def child_task(self):
'''Launch `mavfft_display`'''
# run the fft tool
mavfft_display(self.mlog, self.xlimits.timestamp_in_range)
def mavfft_display(mlog, timestamp_in_range):
'''display fft for raw ACC data in logfile'''
'''object to store data about a single FFT plot'''
class PlotData(object):
def __init__(self, ffth):
self.seqno = -1
self.fftnum = ffth.N
self.sensor_type = ffth.type
self.instance = ffth.instance
self.sample_rate_hz = ffth.smp_rate
self.multiplier = ffth.mul
self.data = {}
self.data["X"] = []
self.data["Y"] = []
self.data["Z"] = []
self.holes = False
self.freq = None
def add_fftd(self, fftd):
if fftd.N != self.fftnum:
print("Skipping ISBD with wrong fftnum (%u vs %u)\n" % (fftd.fftnum, self.fftnum))
return
if self.holes:
print("Skipping ISBD(%u) for ISBH(%u) with holes in it" % (fftd.seqno, self.fftnum))
return
if fftd.seqno != self.seqno+1:
print("ISBH(%u) has holes in it" % fftd.N)
self.holes = True
return
self.seqno += 1
self.data["X"].extend(fftd.x)
self.data["Y"].extend(fftd.y)
self.data["Z"].extend(fftd.z)
def prefix(self):
if self.sensor_type == 0:
return "Accel"
elif self.sensor_type == 1:
return "Gyro"
else:
return "?Unknown Sensor Type?"
def tag(self):
return str(self)
def __str__(self):
return "%s[%u]" % (self.prefix(), self.instance)
print("Processing log for ISBH and ISBD messages")
things_to_plot = []
plotdata = None
start_time = time.time()
mlog.rewind()
while True:
m = mlog.recv_match(type=['ISBH','ISBD'])
if m is None:
break
in_range = timestamp_in_range(m._timestamp)
if in_range < 0:
continue
if in_range > 0:
break
msg_type = m.get_type()
if msg_type == "ISBH":
if plotdata is not None:
# close off previous data collection
things_to_plot.append(plotdata)
# initialise plot-data collection object
plotdata = PlotData(m)
continue
if msg_type == "ISBD":
if plotdata is None:
continue
plotdata.add_fftd(m)
if len(things_to_plot) == 0:
print("No FFT data. Did you set INS_LOG_BAT_MASK?")
return
time_delta = time.time() - start_time
print("Extracted %u fft data sets" % len(things_to_plot))
sum_fft = {}
freqmap = {}
count = 0
first_freq = None
for thing_to_plot in things_to_plot:
for axis in [ "X","Y","Z" ]:
d = numpy.array(thing_to_plot.data[axis])/float(thing_to_plot.multiplier)
if len(d) == 0:
print("No data?!?!?!")
continue
avg = numpy.sum(d) / len(d)
d -= avg
d_fft = numpy.fft.rfft(d)
if thing_to_plot.tag() not in sum_fft:
sum_fft[thing_to_plot.tag()] = {
"X": 0,
"Y": 0,
"Z": 0
}
sum_fft[thing_to_plot.tag()][axis] = numpy.add(sum_fft[thing_to_plot.tag()][axis], d_fft)
count += 1
freq = numpy.fft.rfftfreq(len(d), 1.0/thing_to_plot.sample_rate_hz)
freqmap[thing_to_plot.tag()] = freq
for sensor in sum_fft:
pylab.figure(str(sensor))
for axis in [ "X","Y","Z" ]:
pylab.plot(freqmap[sensor], numpy.abs(sum_fft[sensor][axis]/count), label=axis)
pylab.legend(loc='upper right')
pylab.xlabel('Hz')
pylab.show()
MAVProxy-1.8.74/MAVProxy/modules/lib/mission_item_protocol.py 0000664 0000000 0000000 00000124231 15043245150 0024177 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''
base class for modules generally transfering items using the MISSION_ITEM protocol
AP_FLAKE8_CLEAN
'''
import copy
import os
import re
import struct
import sys
import time
from pymavlink import mavutil
from MAVProxy.modules.lib import mp_module
from MAVProxy.modules.lib import mp_util
if mp_util.has_wxpython:
from MAVProxy.modules.lib.mp_menu import MPMenuCallFileDialog
from MAVProxy.modules.lib.mp_menu import MPMenuItem
from MAVProxy.modules.lib.mp_menu import MPMenuSubMenu
try:
# py2
from StringIO import StringIO as SIO
except ImportError:
# py3
from io import BytesIO as SIO
class MissionItemProtocolModule(mp_module.MPModule):
def __init__(self, mpstate, name, description, **args):
super(MissionItemProtocolModule, self).__init__(mpstate, name, description, **args)
self.add_command(self.command_name(),
self.cmd_wp,
'%s management' % self.itemtype(),
self.completions())
self.wp_op = None
self.wp_requested = {}
self.wp_received = {}
self.wp_save_filename = None
self.wploader_by_sysid = {}
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.undo_wp = None
self.undo_type = None
self.undo_wp_idx = -1
self.upload_start = None
self.last_get_home = time.time()
self.ftp_count = None
if self.continue_mode and self.logdir is not None:
waytxt = os.path.join(mpstate.status.logdir, self.save_filename())
if os.path.exists(waytxt):
self.wploader.load(waytxt)
print("Loaded %s from %s" % (self.itemstype(), waytxt))
self.init_gui_menus()
def gui_menu_items(self):
return [
MPMenuItem('FTP', 'FTP', '# %s ftp' % self.command_name()),
MPMenuItem('Clear', 'Clear', '# %s clear' % self.command_name()),
MPMenuItem('List', 'List', '# %s list' % self.command_name()),
MPMenuItem(
'Load', 'Load', '# %s load ' % self.command_name(),
handler=MPMenuCallFileDialog(
flags=('open',),
title='%s Load' % self.mission_type_string(),
wildcard='MissionFiles(*.txt.*.wp,*.waypoints)|*.txt;*.wp;*.waypoints')),
MPMenuItem(
'Save', 'Save', '# %s save ' % self.command_name(),
handler=MPMenuCallFileDialog(
flags=('save', 'overwrite_prompt'),
title='%s Save' % self.mission_type_string(),
wildcard='MissionFiles(*.txt.*.wp,*.waypoints)|*.txt;*.wp;*.waypoints')),
MPMenuItem('Undo', 'Undo', '# %s undo' % self.command_name()),
]
def mission_type_string(self):
loader_class_string = str(self.loader_class())
m = re.search("'([^']*)'", loader_class_string)
if m is None:
raise ValueError("Failed to match %s" % loader_class_string)
cname = m.group(1)
items = cname.split("_")
return items[-1]
def init_gui_menus(self):
'''initialise menus for console and map'''
self.menu_added_console = False
self.menu_added_map = False
self.menu = None
if not mp_util.has_wxpython:
return
self.menu = MPMenuSubMenu(
self.mission_type_string(),
items=self.gui_menu_items()
)
def completions(self):
'''form up MAVProxy-style completion strings used for tab completi
on'''
cs = self.commands()
no_arguments = []
command_argument_buckets = {}
for c in cs:
value = cs[c]
if isinstance(value, tuple):
(function, arguments) = value
args_string = " ".join(arguments)
if args_string not in command_argument_buckets:
command_argument_buckets[args_string] = []
command_argument_buckets[args_string].append(c)
else:
no_arguments.append(c)
ret = []
if len(no_arguments):
ret.append("<" + "|".join(sorted(no_arguments)) + ">")
for k in command_argument_buckets:
ret.append("<" + "|".join(sorted(command_argument_buckets[k])) + "> " + k)
return ret
def unload(self):
self.remove_command(self.command_name())
self.unload_remove_menu_items()
def unload_remove_menu_items(self):
'''remove out menu items from other modules'''
if self.menu is None:
'''can get here if wxpython is not present'''
return
if self.module('console') is not None and self.menu_added_console:
self.menu_added_console = False
self.module('console').remove_menu(self.menu)
if self.module('map') is not None and self.menu_added_map:
self.menu_added_map = False
self.module('map').remove_menu(self.menu)
super(MissionItemProtocolModule, self).unload()
def create_loader(self):
c = self.loader_class()
return c()
def last_change(self):
return self.wploader.last_change
def check_have_list(self):
if self.last_change() == 0:
print("Please list %s items first" % self.command_name())
return False
return True
def index_from_0(self):
'''e.g. rally points etc are indexed from 1 from the user interface
perspective'''
return False
def save_filename_base(self):
return self.itemstype().replace(" ", "-")
def save_filename(self):
return self.save_filename_base() + ".txt"
@property
def wploader(self):
'''per-sysid wploader'''
if self.target_system not in self.wploader_by_sysid:
self.wploader_by_sysid[self.target_system] = self.create_loader()
self.wploader_by_sysid[self.target_system].expected_count = 0
return self.wploader_by_sysid[self.target_system]
def good_item_num_to_manipulate(self, idx):
if idx > self.wploader.count():
return False
if idx < 1:
return False
return True
def item_num_to_offset(self, item_num):
if self.index_from_0():
return item_num
return item_num - 1
def missing_wps_to_request(self):
ret = []
tnow = time.time()
next_seq = self.wploader.count()
for i in range(5):
seq = next_seq+i
if seq+1 > self.wploader.expected_count:
continue
if seq in self.wp_requested and tnow - self.wp_requested[seq] < 2:
continue
ret.append(seq)
return ret
def append(self, item):
'''append an item to the held item list'''
if not self.check_have_list():
return
if isinstance(item, list):
for i in item:
self.wploader.add(i)
self.wploader.expected_count += 1
else:
self.wploader.add(item)
self.wploader.expected_count += 1
self.wploader.last_change = time.time()
self.wploader.reindex()
def send_wp_requests(self, wps=None):
'''send some more WP requests'''
if wps is None:
wps = self.missing_wps_to_request()
tnow = time.time()
for seq in wps:
self.wp_requested[seq] = tnow
if self.settings.wp_use_mission_int:
method = self.master.mav.mission_request_int_send
else:
method = self.master.mav.mission_request_send
method(self.target_system,
self.target_component,
seq,
mission_type=self.mav_mission_type())
def cmd_status(self, args):
'''show status of wp download'''
if not self.check_have_list():
return
try:
print("Have %u of %u %s" % (
self.wploader.count()+len(self.wp_received),
self.wploader.expected_count,
self.itemstype()))
except Exception:
print("Have %u %s" % (self.wploader.count()+len(self.wp_received), self.itemstype()))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
mtype = m.get_type()
if mtype in ['MISSION_COUNT']:
if getattr(m, 'mission_type', 0) != self.mav_mission_type():
return
if self.wp_op is None:
if self.wploader.expected_count != m.count:
self.console.writeln("Mission is stale")
else:
self.wploader.clear()
self.console.writeln("Requesting %u %s t=%s now=%s" % (
m.count,
self.itemstype(),
time.asctime(time.localtime(m._timestamp)),
time.asctime()))
self.wploader.expected_count = m.count
self.send_wp_requests()
elif mtype in ['MISSION_ITEM', 'MISSION_ITEM_INT'] and self.wp_op is not None:
if m.get_type() == 'MISSION_ITEM_INT':
if getattr(m, 'mission_type', 0) != self.mav_mission_type():
# this is not a mission item, likely fence
return
# our internal structure assumes MISSION_ITEM'''
m = self.wp_from_mission_item_int(m)
if m.seq < self.wploader.count():
# print("DUPLICATE %u" % m.seq)
return
if m.seq+1 > self.wploader.expected_count:
self.console.writeln("Unexpected %s number %u - expected %u" % (self.itemtype(), m.seq, self.wploader.count()))
self.wp_received[m.seq] = m
next_seq = self.wploader.count()
while next_seq in self.wp_received:
m = self.wp_received.pop(next_seq)
self.wploader.add(m)
next_seq += 1
if self.wploader.count() != self.wploader.expected_count:
# print("m.seq=%u expected_count=%u" % (m.seq, self.wploader.expected_count))
self.send_wp_requests()
return
if self.wp_op == 'list':
self.show_and_save(m.get_srcSystem())
self.loading_waypoints = False
elif self.wp_op == "save":
self.save_waypoints(self.wp_save_filename)
self.wp_op = None
self.wp_requested = {}
self.wp_received = {}
elif mtype in frozenset(["MISSION_REQUEST", "MISSION_REQUEST_INT"]):
self.process_waypoint_request(m, self.master)
def idle_task(self):
'''handle missing waypoints'''
if self.wp_period.trigger():
# cope with packet loss fetching mission
if (self.master is not None and
self.master.time_since('MISSION_ITEM') >= 2 and
self.wploader.count() < getattr(self.wploader, 'expected_count', 0)):
wps = self.missing_wps_to_request()
print("re-requesting %s %s" % (self.itemstype(), str(wps)))
self.send_wp_requests(wps)
self.idle_task_add_menu_items()
def idle_task_add_menu_items(self):
'''check for load of other modules, add our items as required'''
if self.menu is None:
'''can get here if wxpython is not present'''
return
if self.module('console') is not None:
if not self.menu_added_console:
self.menu_added_console = True
self.module('console').add_menu(self.menu)
else:
self.menu_added_console = False
if self.module('map') is not None:
if not self.menu_added_map:
self.menu_added_map = True
self.module('map').add_menu(self.menu)
else:
self.menu_added_map = False
def has_location(self, cmd_id):
'''return True if a WP command has a location'''
if cmd_id in mavutil.mavlink.enums['MAV_CMD'].keys():
cmd_enum = mavutil.mavlink.enums['MAV_CMD'][cmd_id]
# default to having location for older installs of pymavlink
# which don't have the attribute
return getattr(cmd_enum, 'has_location', True)
return False
def wp_to_mission_item_int(self, wp):
'''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as
MISSION_ITEM_INT to give cm level accuracy
'''
if wp.get_type() == 'MISSION_ITEM_INT':
if not isinstance(wp.x, int):
print("BUG! wp.x is not integer")
if not isinstance(wp.y, int):
print("BUG! wp.y is not integer")
return wp
if self.has_location(wp.command):
p5 = int(wp.x*1.0e7)
p6 = int(wp.y*1.0e7)
else:
p5 = int(wp.x)
p6 = int(wp.y)
wp_int = mavutil.mavlink.MAVLink_mission_item_int_message(
wp.target_system,
wp.target_component,
wp.seq,
wp.frame,
wp.command,
wp.current,
wp.autocontinue,
wp.param1,
wp.param2,
wp.param3,
wp.param4,
p5,
p6,
wp.z,
wp.mission_type
)
return wp_int
def wp_from_mission_item_int(self, wp):
'''convert a MISSION_ITEM_INT to a MISSION_ITEM'''
if self.has_location(wp.command):
p5 = wp.x*1.0e-7
p6 = wp.y*1.0e-7
else:
p5 = wp.x
p6 = wp.y
wp2 = mavutil.mavlink.MAVLink_mission_item_message(wp.target_system,
wp.target_component,
wp.seq,
wp.frame,
wp.command,
wp.current,
wp.autocontinue,
wp.param1,
wp.param2,
wp.param3,
wp.param4,
p5,
p6,
wp.z,
wp.mission_type)
# preserve srcSystem as that is used for naming waypoint file
wp2._header.srcSystem = wp.get_srcSystem()
wp2._header.srcComponent = wp.get_srcComponent()
return wp2
def process_waypoint_request(self, m, master):
'''process a waypoint request from the master'''
if m.mission_type != self.mav_mission_type():
return
# print("Processing %s request: (%s)" % (self.itemtype(), str(m)))
if (m.target_system != self.settings.source_system or
m.target_component != self.settings.source_component):
# self.console.error("Mission request is not for me")
return
if (not self.loading_waypoints or
time.time() > self.loading_waypoint_lasttime + 10.0):
self.loading_waypoints = False
# self.console.error("not loading waypoints")
return
if m.seq >= self.wploader.count():
self.console.error("Request for bad %s %u (max %u)" %
(self.itemtype, m.seq, self.wploader.count()))
return
wp = self.wploader.wp(m.seq)
wp.target_system = self.target_system
wp.target_component = self.target_component
if self.settings.wp_use_mission_int:
wp_send = self.wp_to_mission_item_int(wp)
else:
wp_send = wp
if wp.mission_type != self.mav_mission_type():
print("Wrong mission type in (%s)" % str(wp))
try:
self.master.mav.send(wp_send)
except struct.error:
# this is often "required argument is not an integer"
mavutil.dump_message_verbose(sys.stderr, wp_send)
raise
self.loading_waypoint_lasttime = time.time()
# update the user on our progress:
self.mpstate.console.set_status(self.itemtype(), '%s %u/%u' % (self.itemtype(), m.seq, self.wploader.count()-1))
# see if the transfer is complete:
if m.seq == self.wploader.count() - 1:
self.loading_waypoints = False
print("Loaded %u %s in %.2fs" % (
self.wploader.count(),
self.itemstype(),
time.time() - self.upload_start))
self.console.writeln(
"Sent all %u %s" %
(self.wploader.count(), self.itemstype()))
def send_all_waypoints(self):
return self.send_all_items()
def send_all_items(self):
'''send all waypoints to vehicle'''
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.upload_start = time.time()
self.master.mav.mission_count_send(
self.target_system,
self.target_component,
self.wploader.count(),
mission_type=self.mav_mission_type())
def load_waypoints(self, filename):
'''load waypoints from a file'''
self.wploader.target_system = self.target_system
self.wploader.target_component = self.target_component
try:
# need to remove the leading and trailing quotes in filename
self.wploader.load(filename.strip('"'))
except Exception as msg:
print("Unable to load %s - %s" % (filename, msg))
return
print("Loaded %u %s from %s" % (self.wploader.count(), self.itemstype(), filename))
self.wploader.expected_count = self.wploader.count()
self.send_all_waypoints()
def update_waypoints(self, filename, wpnum):
'''update waypoints from a file'''
self.wploader.target_system = self.target_system
self.wploader.target_component = self.target_component
try:
self.wploader.load(filename)
except Exception as msg:
print("Unable to load %s - %s" % (filename, msg))
return
if self.wploader.count() == 0:
print("No %s found in %s" % (self.itemstype(), filename))
return
if wpnum == -1:
print("Loaded %u updated %s from %s" % (self.wploader.count(), self.itemstype(), filename))
elif wpnum >= self.wploader.count():
print("Invalid %s number %u" % (self.itemtype(), wpnum))
return
else:
print("Loaded updated %s %u from %s" % (self.itemtype(), wpnum, filename))
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
if wpnum == -1:
start = 0
end = self.wploader.count()-1
else:
start = wpnum
end = wpnum
self.master.mav.mission_write_partial_list_send(
self.target_system,
self.target_component,
start,
end,
self.mav_mission_type())
def save_waypoints(self, filename):
'''save waypoints to a file'''
try:
# need to remove the leading and trailing quotes in filename
self.wploader.save(filename.strip('"'))
except Exception as msg:
print("Failed to save %s - %s" % (filename, msg))
return
print("Saved %u %s to %s" % (self.wploader.count(), self.itemstype(), filename))
def save_waypoints_csv(self, filename):
'''save waypoints to a file in a human readable CSV file'''
try:
# need to remove the leading and trailing quotes in filename
self.wploader.savecsv(filename.strip('"'))
except Exception as msg:
print("Failed to save %s - %s" % (filename, msg))
return
print("Saved %u %s to CSV %s" % (self.wploader.count(), self.itemstype(), filename))
def cmd_move(self, args):
'''handle wp move'''
if len(args) != 1:
print("usage: wp move WPNUM")
return
idx = int(args[0])
if not self.good_item_num_to_manipulate(idx):
print("Invalid %s number %u" % (self.itemtype(), idx))
return
offset = self.item_num_to_offset(idx)
latlon = self.mpstate.click_location
if latlon is None:
print("No map click position available")
return
wp = self.wploader.wp(offset)
# setup for undo
self.undo_wp = copy.copy(wp)
self.undo_wp_idx = idx
self.undo_type = "move"
(lat, lon) = latlon
if (len(self.module_matching('terrain')) > 0 and
wp.frame == mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT and
self.settings.wpterrainadjust):
alt1 = self.module('terrain').ElevationModel.GetElevation(lat, lon)
alt2 = self.module('terrain').ElevationModel.GetElevation(wp.x, wp.y)
if alt1 is not None and alt2 is not None:
wp.z += alt1 - alt2
if isinstance(wp, mavutil.mavlink.MAVLink_mission_item_int_message):
wp.x = int(lat * 1e7)
wp.y = int(lon * 1e7)
else:
wp.x = lat
wp.y = lon
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, offset)
self.wploader.last_change = time.time()
print("Moving %s %u to %f, %f at %.1fm" % (self.itemtype(), idx, lat, lon, wp.z))
self.send_single_waypoint(offset)
def send_single_waypoint(self, idx):
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.upload_start = time.time()
self.master.mav.mission_write_partial_list_send(
self.target_system,
self.target_component,
idx,
idx,
self.mav_mission_type()
)
def is_location_command(self, cmd):
'''see if cmd is a MAV_CMD with a latitude/longitude'''
mav_cmd = mavutil.mavlink.enums['MAV_CMD']
if cmd not in mav_cmd:
return False
return getattr(mav_cmd[cmd], 'has_location', True)
def is_location_wp(self, w):
'''see if w.command is a MAV_CMD with a latitude/longitude'''
if w.x == 0 and w.y == 0:
return False
return self.is_location_command(w.command)
def cmd_movemulti(self, args, latlon=None):
'''handle wp move of multiple waypoints'''
if len(args) < 3:
print("usage: wp movemulti WPNUM WPSTART WPEND ")
return
idx = int(args[0])
if not self.good_item_num_to_manipulate(idx):
print("Invalid move %s number %u" % (self.itemtype(), idx))
return
wpstart = int(args[1])
if not self.good_item_num_to_manipulate(wpstart):
print("Invalid start %s number %u" % (self.itemtype(), wpstart))
return
wpend = int(args[2])
if not self.good_item_num_to_manipulate(wpend):
print("Invalid end %s number %u" % (self.itemtype(), wpend))
return
if idx < wpstart or idx > wpend:
print("WPNUM must be between WPSTART and WPEND")
return
# optional rotation about center point
if len(args) > 3:
rotation = float(args[3])
else:
rotation = 0
if latlon is None:
latlon = self.mpstate.click_location
if latlon is None:
print("No map click position available")
return
idx_offset = self.item_num_to_offset(idx)
wp = self.wploader.wp(idx_offset)
if not self.is_location_wp(wp):
print("WP must be a location command")
return
(lat, lon) = latlon
distance = mp_util.gps_distance(wp.x, wp.y, lat, lon)
bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon)
wpstart_offset = self.item_num_to_offset(wpstart)
wpend_offset = self.item_num_to_offset(wpend)
for wpnum in range(wpstart_offset, wpend_offset+1):
wp = self.wploader.wp(wpnum)
if wp is None or not self.is_location_wp(wp):
continue
(newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing, distance)
if wpnum != idx and rotation != 0:
# add in rotation
d2 = mp_util.gps_distance(lat, lon, newlat, newlon)
b2 = mp_util.gps_bearing(lat, lon, newlat, newlon)
(newlat, newlon) = mp_util.gps_newpos(lat, lon, b2+rotation, d2)
if (len(self.module_matching('terrain')) > 0 and
wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT and
self.settings.wpterrainadjust):
alt1 = self.module('terrain').ElevationModel.GetElevation(newlat, newlon)
alt2 = self.module('terrain').ElevationModel.GetElevation(wp.x, wp.y)
if alt1 is not None and alt2 is not None:
wp.z += alt1 - alt2
wp.x = newlat
wp.y = newlon
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, wpnum)
self.wploader.last_change = time.time()
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.master.mav.mission_write_partial_list_send(
self.target_system,
self.target_component,
wpstart_offset,
wpend_offset)
print("Moved %s %u:%u to %f, %f rotation=%.1f" % (self.itemstype(), wpstart, wpend, lat, lon, rotation))
def change_mission_item_range(self, args, desc, changer, newvalstr):
if not self.check_have_list():
return
idx = int(args[0])
if not self.good_item_num_to_manipulate(idx):
print("Invalid %s number %u" % (self.itemtype(), idx))
return
if len(args) >= 2:
count = int(args[1])
else:
count = 1
if not self.good_item_num_to_manipulate(idx+count-1):
print("Invalid %s number %u" % (self.itemtype(), idx+count-1))
return
for wpnum in range(idx, idx+count):
offset = self.item_num_to_offset(wpnum)
wp = self.wploader.wp(offset)
if wp is None:
continue
if not self.wploader.is_location_command(wp.command):
continue
changer(wp)
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, offset)
self.wploader.last_change = time.time()
self.upload_start = time.time()
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.master.mav.mission_write_partial_list_send(
self.target_system,
self.target_component,
self.item_num_to_offset(idx),
self.item_num_to_offset(idx+count),
mission_type=self.mav_mission_type())
print("Changed %s for WPs %u:%u to %s" % (desc, idx, idx+(count-1), newvalstr))
def cmd_changealt(self, args):
'''handle wp change target alt of multiple waypoints'''
if len(args) < 2:
print("usage: %s changealt WPNUM NEWALT " % self.command_name())
return
value = float(args[1])
del args[1]
def changer(wp):
wp.z = value
self.change_mission_item_range(args, "alt", changer, str(value))
def cmd_changeframe(self, args):
'''handle wp change frame of multiple waypoints'''
if len(args) < 2:
print("usage: %s changeframe WPNUM NEWFRAME " % self.command_name())
return
value = int(args[1])
del args[1]
def changer(wp):
wp.frame = value
self.change_mission_item_range(args, "frame", changer, str(value))
def fix_jumps(self, idx, delta):
# nothing by default as only waypoints need worry
pass
def cmd_remove(self, args):
'''handle wp remove'''
if len(args) != 1:
print("usage: %s remove WPNUM" % self.command_name())
return
idx = int(args[0])
if not self.good_item_num_to_manipulate(idx):
print("Invalid %s number %u" % (self.itemtype(), idx))
return
offset = self.item_num_to_offset(idx)
wp = self.wploader.wp(offset)
# setup for undo
self.undo_wp = copy.copy(wp)
self.undo_wp_idx = idx
self.undo_type = "remove"
self.wploader.remove(wp)
self.wploader.expected_count -= 1
self.wploader.last_change = time.time()
self.fix_jumps(offset, -1)
self.send_all_waypoints()
print("Removed %s %u" % (self.itemtype(), idx))
def cmd_undo(self, args):
'''handle wp undo'''
if self.undo_wp_idx == -1 or self.undo_wp is None:
print("No undo information")
return
wp = self.undo_wp
if self.undo_type == 'move':
wp.target_system = self.target_system
wp.target_component = self.target_component
offset = self.item_num_to_offset(self.undo_wp_idx)
self.wploader.set(wp, offset)
self.wploader.last_change = time.time()
self.send_single_waypoint(offset)
print("Undid %s move" % self.itemtype())
elif self.undo_type == 'remove':
offset = self.item_num_to_offset(self.undo_wp_idx)
self.wploader.insert(offset, wp)
self.wploader.expected_count += 1
self.wploader.last_change = time.time()
self.fix_jumps(self.undo_wp_idx, 1)
self.send_all_waypoints()
print("Undid %s remove" % self.itemtype())
else:
print("bad undo type")
self.undo_wp = None
self.undo_wp_idx = -1
def cmd_param(self, args):
'''handle wp parameter change'''
if len(args) < 2:
print("usage: wp param WPNUM PNUM ")
return
idx = int(args[0])
if not self.good_item_num_to_manipulate(idx):
print("Invalid %s number %u" % (self.itemtype(), idx))
return
offset = self.item_num_to_offset(idx)
wp = self.wploader.wp(offset)
param = [wp.param1, wp.param2, wp.param3, wp.param4]
pnum = int(args[1])
if pnum < 1 or pnum > 4:
print("Invalid param number %u" % pnum)
return
if len(args) == 2:
print("Param %u: %f" % (pnum, param[pnum-1]))
return
param[pnum-1] = float(args[2])
wp.param1 = param[0]
wp.param2 = param[1]
wp.param3 = param[2]
wp.param4 = param[3]
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, idx)
self.wploader.last_change = time.time()
self.send_single_waypoint(idx)
def cmd_clear(self, args):
self.master.mav.mission_clear_all_send(
self.target_system,
self.target_component,
mission_type=self.mav_mission_type())
self.wploader.clear()
if getattr(self.wploader, 'expected_count', None) is not None:
self.wploader.expected_count = 0
self.wploader.expected_count = 0
self.loading_waypoint_lasttime = time.time()
def cmd_list(self, args):
self.wp_op = "list"
self.request_list_send()
def cmd_load(self, args):
if len(args) != 1:
print("usage: %s load FILENAME" % self.command_name())
return
self.load_waypoints(args[0])
def cmd_save(self, args):
if len(args) != 1:
print("usage: %s save " % self.command_name())
return
self.wp_save_filename = args[0]
self.wp_op = "save"
self.request_list_send()
def cmd_savecsv(self, args):
if len(args) != 1:
print("usage: wp savecsv ")
return
self.savecsv(args[0])
def cmd_savelocal(self, args):
if len(args) != 1:
print("usage: wp savelocal ")
return
self.wploader.save(args[0])
def cmd_show(self, args):
if len(args) != 1:
print("usage: wp show ")
return
self.wploader.load(args[0])
def cmd_update(self, args):
if not self.check_have_list():
return
if len(args) < 1:
print("usage: %s update " % self.command_name())
return
if len(args) == 2:
wpnum = int(args[1])
else:
wpnum = -1
self.update_waypoints(args[0], wpnum)
def commands(self):
if self.master and not self.master.mavlink20():
print("%s module not available; use old compat modules" % str(self.itemtype()))
return
return {
"ftp": self.wp_ftp_download,
"ftpload": self.wp_ftp_upload,
"clear": self.cmd_clear,
"list": self.cmd_list,
"load": (self.cmd_load, ["(FILENAME)"]),
"remove": self.cmd_remove,
"save": (self.cmd_save, ["(FILENAME)"]),
"savecsv": (self.cmd_savecsv, ["(FILENAME)"]),
"savelocal": self.cmd_savelocal,
"show": (self.cmd_show, ["(FILENAME)"]),
"status": self.cmd_status,
}
def usage(self):
subcommands = "|".join(sorted(self.commands().keys()))
return "usage: %s <%s>" % (self.command_name(), subcommands)
def cmd_wp(self, args):
'''waypoint commands'''
if len(args) < 1:
print(self.usage())
return
commands = self.commands()
if args[0] not in commands:
print(self.usage())
return
function = commands[args[0]]
if isinstance(function, tuple):
(function, function_arguments) = function
# TODO: do some argument validation here, remove same from
# cmd_*
function(args[1:])
def pretty_enum_value(self, enum_name, enum_value):
if enum_name == "MAV_FRAME":
if enum_value == 0:
return "Abs"
elif enum_value == 1:
return "Local"
elif enum_value == 2:
return "Mission"
elif enum_value == 3:
return "Rel"
elif enum_value == 4:
return "Local ENU"
elif enum_value == 5:
return "Global (INT)"
elif enum_value == 10:
return "AGL"
ret = mavutil.mavlink.enums[enum_name][enum_value].name
ret = ret[len(enum_name)+1:]
return ret
def csv_line(self, line):
'''turn a list of values into a CSV line'''
self.csv_sep = ","
return self.csv_sep.join(['"' + str(x) + '"' for x in line])
def pretty_parameter_value(self, value):
'''pretty parameter value'''
return value
def savecsv(self, filename):
'''save waypoints to a file in human-readable CSV file'''
f = open(filename, mode='w')
# headers = ["Seq", "Frame", "Cmd", "P1", "P2", "P3", "P4", "X", "Y", "Z"]
for w in self.wploader.wpoints:
if getattr(w, 'comment', None):
# f.write("# %s\n" % w.comment)
pass
out_list = [
w.seq,
self.pretty_enum_value('MAV_FRAME', w.frame),
self.pretty_enum_value('MAV_CMD', w.command),
self.pretty_parameter_value(w.param1),
self.pretty_parameter_value(w.param2),
self.pretty_parameter_value(w.param3),
self.pretty_parameter_value(w.param4),
self.pretty_parameter_value(w.x),
self.pretty_parameter_value(w.y),
self.pretty_parameter_value(w.z),
]
f.write(self.csv_line(out_list) + "\n")
f.close()
def fetch(self):
"""Download wpts from vehicle (this operation is public to support other modules)"""
if self.wp_op is None: # If we were already doing a list or save, just restart the fetch without changing the operation # noqa
self.wp_op = "fetch"
self.request_list_send()
def request_list_send(self):
self.master.mav.mission_request_list_send(
self.target_system,
self.target_component,
mission_type=self.mav_mission_type())
def wp_ftp_download(self, args):
'''Download items from vehicle with ftp'''
ftp = self.mpstate.module('ftp')
if ftp is None:
print("Need ftp module")
return
self.ftp_count = None
ftp.cmd_get([self.mission_ftp_name()], callback=self.ftp_callback, callback_progress=self.ftp_callback_progress)
def ftp_callback_progress(self, fh, total_size):
'''progress callback from ftp fetch of mission items'''
if self.ftp_count is None and total_size >= 10:
ofs = fh.tell()
fh.seek(0)
buf = fh.read(10)
fh.seek(ofs)
magic2, dtype, options, start, num_items = struct.unpack("= item_size:
mdata = data[:item_size]
data = data[item_size:]
msg = mavmsg.unpacker.unpack(mdata)
tlist = list(msg)
t = tlist[:]
for i in range(0, len(tlist)):
tlist[i] = t[mavmsg.orders[i]]
t = tuple(tlist)
w = mavmsg(*t)
w = self.wp_from_mission_item_int(w)
self.wploader.add(w)
self.show_and_save(self.target_system)
def show_and_save(self, source_system):
'''display waypoints and save'''
for i in range(self.wploader.count()):
w = self.wploader.wp(i)
print("%u %u %.10f %.10f %f p1=%.1f p2=%.1f p3=%.1f p4=%.1f cur=%u auto=%u" % (
w.command, w.frame, w.x, w.y, w.z,
w.param1, w.param2, w.param3, w.param4,
w.current, w.autocontinue))
if self.logdir is not None:
fname = self.save_filename()
if source_system != 1:
fname = '%s_%u.txt' % (self.save_filename_base(), source_system)
waytxt = os.path.join(self.logdir, fname)
self.save_waypoints(waytxt)
print("Saved %s to %s" % (self.itemstype(), waytxt))
def wp_ftp_upload(self, args):
'''upload waypoints to vehicle with ftp'''
filename = args[0]
ftp = self.mpstate.module('ftp')
if ftp is None:
print("Need ftp module")
return
self.wploader.target_system = self.target_system
self.wploader.target_component = self.target_component
try:
# need to remove the leading and trailing quotes in filename
self.wploader.load(filename.strip('"'))
except Exception as msg:
print("Unable to load %s - %s" % (filename, msg))
return
print("Loaded %u %s from %s" % (self.wploader.count(), self.itemstype(), filename))
print("Sending %s with ftp" % self.itemstype())
fh = SIO()
fh.write(struct.pack("