cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of https://git.cmtec.se/cm/cmdr-joystick

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-05 13:06:24 +02:00
commit 2003b7d357
11 changed files with 965 additions and 953 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
firmware/.cache/clangd/index
firmware/compile_commands.json

1
CHANGELOG Normal file
View File

@ -0,0 +1 @@
v1.0

339
LICENSE Normal file
View File

@ -0,0 +1,339 @@
GNU GENERAL PUBLIC LICENSE
Version 2, June 1991
Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
License is intended to guarantee your freedom to share and change free
software--to make sure the software is free for all its users. This
General Public License applies to most of the Free Software
Foundation's software and to any other program whose authors commit to
using it. (Some other Free Software Foundation software is covered by
the GNU Lesser General Public License instead.) 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
this service 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 make restrictions that forbid
anyone to deny you these rights or to ask you to surrender the rights.
These restrictions translate to certain responsibilities for you if you
distribute copies of the software, or if you modify it.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must give the recipients all the rights that
you have. 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.
We protect your rights with two steps: (1) copyright the software, and
(2) offer you this license which gives you legal permission to copy,
distribute and/or modify the software.
Also, for each author's protection and ours, we want to make certain
that everyone understands that there is no warranty for this free
software. If the software is modified by someone else and passed on, we
want its recipients to know that what they have is not the original, so
that any problems introduced by others will not reflect on the original
authors' reputations.
Finally, any free program is threatened constantly by software
patents. We wish to avoid the danger that redistributors of a free
program will individually obtain patent licenses, in effect making the
program proprietary. To prevent this, we have made it clear that any
patent must be licensed for everyone's free use or not licensed at all.
The precise terms and conditions for copying, distribution and
modification follow.
GNU GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License applies to any program or other work which contains
a notice placed by the copyright holder saying it may be distributed
under the terms of this General Public License. The "Program", below,
refers to any such program or work, and a "work based on the Program"
means either the Program or any derivative work under copyright law:
that is to say, a work containing the Program or a portion of it,
either verbatim or with modifications and/or translated into another
language. (Hereinafter, translation is included without limitation in
the term "modification".) Each licensee is addressed as "you".
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running the Program is not restricted, and the output from the Program
is covered only if its contents constitute a work based on the
Program (independent of having been made by running the Program).
Whether that is true depends on what the Program does.
1. You may copy and distribute 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 and disclaimer of warranty; keep intact all the
notices that refer to this License and to the absence of any warranty;
and give any other recipients of the Program a copy of this License
along with the Program.
You may charge a fee for the physical act of transferring a copy, and
you may at your option offer warranty protection in exchange for a fee.
2. You may modify your copy or copies of the Program or any portion
of it, thus forming a work based on the Program, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) You must cause the modified files to carry prominent notices
stating that you changed the files and the date of any change.
b) You must cause any work that you distribute or publish, that in
whole or in part contains or is derived from the Program or any
part thereof, to be licensed as a whole at no charge to all third
parties under the terms of this License.
c) If the modified program normally reads commands interactively
when run, you must cause it, when started running for such
interactive use in the most ordinary way, to print or display an
announcement including an appropriate copyright notice and a
notice that there is no warranty (or else, saying that you provide
a warranty) and that users may redistribute the program under
these conditions, and telling the user how to view a copy of this
License. (Exception: if the Program itself is interactive but
does not normally print such an announcement, your work based on
the Program is not required to print an announcement.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Program,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Program, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Program.
In addition, mere aggregation of another work not based on the Program
with the Program (or with a work based on the Program) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may copy and distribute the Program (or a work based on it,
under Section 2) in object code or executable form under the terms of
Sections 1 and 2 above provided that you also do one of the following:
a) Accompany it with the complete corresponding machine-readable
source code, which must be distributed under the terms of Sections
1 and 2 above on a medium customarily used for software interchange; or,
b) Accompany it with a written offer, valid for at least three
years, to give any third party, for a charge no more than your
cost of physically performing source distribution, a complete
machine-readable copy of the corresponding source code, to be
distributed under the terms of Sections 1 and 2 above on a medium
customarily used for software interchange; or,
c) Accompany it with the information you received as to the offer
to distribute corresponding source code. (This alternative is
allowed only for noncommercial distribution and only if you
received the program in object code or executable form with such
an offer, in accord with Subsection b above.)
The source code for a work means the preferred form of the work for
making modifications to it. For an executable work, complete source
code means all the source code for all modules it contains, plus any
associated interface definition files, plus the scripts used to
control compilation and installation of the executable. However, as a
special exception, the source code distributed need not include
anything that is normally distributed (in either source or binary
form) with the major components (compiler, kernel, and so on) of the
operating system on which the executable runs, unless that component
itself accompanies the executable.
If distribution of executable or object code is made by offering
access to copy from a designated place, then offering equivalent
access to copy the source code from the same place counts as
distribution of the source code, even though third parties are not
compelled to copy the source along with the object code.
4. You may not copy, modify, sublicense, or distribute the Program
except as expressly provided under this License. Any attempt
otherwise to copy, modify, sublicense or distribute the Program is
void, and will automatically terminate your rights under this License.
However, parties who have received copies, or rights, from you under
this License will not have their licenses terminated so long as such
parties remain in full compliance.
5. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Program or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Program (or any work based on the
Program), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Program or works based on it.
6. Each time you redistribute the Program (or any work based on the
Program), the recipient automatically receives a license from the
original licensor to copy, distribute or modify the Program subject to
these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties to
this License.
7. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
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
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Program at all. For example, if a patent
license would not permit royalty-free redistribution of the Program by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Program.
If any portion of this section is held invalid or unenforceable under
any particular circumstance, the balance of the section is intended to
apply and the section as a whole is intended to apply in other
circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system, which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
8. If the distribution and/or use of the Program is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Program under this License
may add an explicit geographical distribution limitation excluding
those countries, so that distribution is permitted only in or among
countries not thus excluded. In such case, this License incorporates
the limitation as if written in the body of this License.
9. The Free Software Foundation may publish revised and/or new versions
of the 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 a version number of this License which applies to it and "any
later version", you have the option of following the terms and conditions
either of that version or of any later version published by the Free
Software Foundation. If the Program does not specify a version number of
this License, you may choose any version ever published by the Free Software
Foundation.
10. If you wish to incorporate parts of the Program into other free
programs whose distribution conditions are different, write to the author
to ask for permission. For software which is copyrighted by the Free
Software Foundation, write to the Free Software Foundation; we sometimes
make exceptions for this. Our decision will be guided by the two goals
of preserving the free status of all derivatives of our free software and
of promoting the sharing and reuse of software generally.
NO WARRANTY
11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, 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.
12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
REDISTRIBUTE 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.
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
convey the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
CMDR Joystick
Copyright (C) 2023 Christoffer Martinsson
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 2 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, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
Also add information on how to contact you by electronic and paper mail.
If the program is interactive, make it output a short notice like this
when it starts in an interactive mode:
Gnomovision version 69, Copyright (C) year name of author
Gnomovision 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, the commands you use may
be called something other than `show w' and `show c'; they could even be
mouse-clicks or menu items--whatever suits your program.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the program, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the program
`Gnomovision' (which makes passes at compilers) written by James Hacker.
<signature of Ty Coon>, 1 April 1989
Ty Coon, President of Vice
This 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.

148
README.md
View File

@ -1,92 +1,94 @@
# CMDR Joystick
RC Joystick with 2 hall effect gimbals and 8 buttons for use both with simulators and ELRS Rx equipped quads.
## Layout
## Getting started
```cpp
USB Joystick Layer 0
To make it easy for you to get started with GitLab, here's a list of recommended next steps.
| B3 | | B1 |
| Fn1 | | B2 |
--------------------------------------------
| | B4 | | B5 | |
| |
| X1,Y1 X2,Y2 |
| | B6 | | B7 | |
--------------------------------------------
Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!
USB Joystick Layer 1 (Fn1)
## Add your files
| Fn2 | | B1 |
| Fn1 | | B2 |
--------------------------------------------
| | B8 | | B9 | |
| |
| X1,Y1 X3,Y2 |
| | B10 | | B11 | |
--------------------------------------------
- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files
- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:
USB Joystick Layer 2 (Fn2)
```
cd existing_repo
git remote add origin https://git.cmtec.se/cm/cmdr-joystick.git
git branch -M main
git push -uf origin main
| Fn2 | | B16 |
| Fn1 | | B17 |
--------------------------------------------
| | B12 | | B13 | |
| |
| X1,Y1 X3,Y3 |
| | B14 | | B15 | |
--------------------------------------------
ELRS Layer
| CH6 on | | CH5 on |
| CH6 off | | CH5 off |
--------------------------------------------
| | CH7 | | CH8 | |
| |
| X,Y X,Y |
| CH1,CH2 | CH9 | | CH10 | CH3,CH4 |
--------------------------------------------
```
## Integrate with your tools
## Features
- [ ] [Set up project integrations](https://git.cmtec.se/cm/cmdr-joystick/-/settings/integrations)
- Ergonomic design (low profile)
- Hall effect gimbals
- Supports both USB HID joystick and ELRS Tx module
- Total 6x axis and 15x buttons (using Fn mode) implemented in USB HID mode
- 10 Channels implemented in ELRS mode (4x axis, 6x buttons)
- Low latency (1.6ms ELRS, 5ms USB)
## Collaborate with your team
## Build environment
- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)
- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)
- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)
- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)
- [ ] [Automatically merge when pipeline succeeds](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)
- Platformio
- env: teensylc
- platform: teensy
- board: teensylc
- framework: arduino
- Flashing via Teensy USB bootloader
- Pressing boot button on teensy
- Press and hold "top lower right button" when powering the unit
## Test and Deploy
## Hardware
Use the built-in continuous integration in GitLab.
- 1x TeensyLC MCU
- 2x FrSky M7 or M10 gimbals
- 6x Kailh choc low profile switches
- 2x Cherry MX switches
- 1x PCB
- 1x Bottom case
- 1x Top plate
- 2x Gimbal spacers
- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)
- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing(SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)
- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)
- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)
- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)
## Calibration
***
_The button is from here reffered to "top lower left button"_
# Editing this README
When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thank you to [makeareadme.com](https://www.makeareadme.com/) for this template.
## Suggestions for a good README
Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.
## Name
Choose a self-explaining name for your project.
## Description
Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.
## Badges
On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.
## Visuals
Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.
## Installation
Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.
## Usage
Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.
## Support
Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.
## Roadmap
If you have ideas for releases in the future, it is a good idea to list them in the README.
## Contributing
State if you are open to contributions and what your requirements are for accepting them.
For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.
You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.
## Authors and acknowledgment
Show your appreciation to those who have contributed to the project.
## License
For open source projects, say how it is licensed.
## Project status
If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.
1. Turn off the unit
2. Press and hold the button while powering the unit
3. Release the button and center the two gimbals
4. Press the button again
5. Move the two gimbals to it maximux X and Y
6. Press the button one mo time
7. Done!

2
firmware/.gitignore vendored
View File

@ -1,2 +1,4 @@
.pio
.vscode
.cache
compile_commands.json

20
firmware/Makefile
View File

@ -3,23 +3,11 @@
#PATH := /usr/local/bin:$(PATH)
all:
platformio run
upload:
platformio run --target upload
pio run && pio run -t compiledb
clean:
platformio run --target clean
pio run -t clean
program:
platformio run --target program
upload:
pio run -t upload
uploadfs:
platformio run --target uploadfs
update:
platformio update
bear:
platformio run --target clean
bear make all

5
firmware/extra_script.py Normal file
View File

@ -0,0 +1,5 @@
import os
Import("env")
# include toolchain paths
env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True)

1
firmware/lib/teensy-libc Submodule

@ -0,0 +1 @@
Subproject commit 88cb7c10ca12ea33fb96aa10ae97a14d087da852

4
firmware/platformio.ini
View File

@ -12,5 +12,7 @@
platform = teensy
board = teensylc
framework = arduino
build_flags = -D USB_HID -D LAYOUT_SWEDISH -w
build_flags = -Isrc -Ilib -D USB_HID -D LAYOUT_SWEDISH -w
board_build.f_cpu = 48000000L
extra_scripts = pre:extra_script.py
upload_protocol = teensy-cli

533
firmware/src/cmdr_joystick.cpp Normal file
View File

@ -0,0 +1,533 @@
/*
* Project: CMtec CMDR Joystick RC
* Date: 2023-09-02
* Author: Christoffer Martinsson
* Email: cm@cmtec.se
* License: Please refer to LICENSE file in root folder
*/
#include <Arduino.h>
#include <EEPROM.h>
#include <ElrsTx.h>
#include <IndicatorLed.h>
#include <ResponsiveAnalogRead.h>
IndicatorLed status_led(13);
IndicatorLed button_led_1(22);
IndicatorLed button_led_2(23);
const int TIME_US_1ms = 1000;
const int TIME_US_5ms = 5000;
const uint64_t TIME_US_200ms = 200000;
uint64_t current_timestamp_micros = 0;
uint64_t process_data_timestamp_micros = 0;
uint64_t indicator_timestamp_micros = 0;
uint64_t send_usb_timestamp_micros = 0;
uint64_t send_elrs_timestamp_micros = 0;
const int BUTTON_PRESSED = LOW;
const int BUTTON_NOT_PRESSED = HIGH;
const int BUTTON_FRONT_LEFT_UPPER_PIN = 4;
const int BUTTON_FRONT_LEFT_LOWER_PIN = 5;
const int BUTTON_FRONT_RIGHT_UPPER_PIN = 6;
const int BUTTON_FRONT_RIGHT_LOWER_PIN = 7;
const int BUTTON_TOP_LEFT_UPPER_PIN = 9;
const int BUTTON_TOP_RIGHT_UPPER_PIN = 10;
const int BUTTON_TOP_LEFT_LOWER_PIN = 8;
const int BUTTON_TOP_RIGHT_LOWER_PIN = 11;
const int GIMBAL_MODE_FRSKY_M7 = 0;
const int GIMBAL_MODE_FRSKY_M10 = 1;
const int GIMBAL_MODE_PIN = 21;
int gimbal_mode = GIMBAL_MODE_FRSKY_M7;
bool toggle_button_arm = false;
bool toggle_button_mode = false;
const int AXIS_10BIT_MAX = 1023;
const int AXIS_10BIT_MIN = 0;
const int AXIS_10BIT_CENTER = 512;
const int AXIS_12BIT_MAX = 4096;
const int AXIS_12BIT_MIN = 0;
const int AXIS_12BIT_CENTER = 2048;
const int DEADZONE_X = 50;
const int DEADZONE_Y = 50;
const int JOYSTICK_HAT_CENTER = -1;
enum EEPROM_ADR {
MAX_X1_ADR_HIGH,
MAX_X1_ADR_LOW,
MIN_X1_ADR_HIGH,
MIN_X1_ADR_LOW,
CNT_X1_ADR_HIGH,
CNT_X1_ADR_LOW,
MAX_Y1_ADR_HIGH,
MAX_Y1_ADR_LOW,
MIN_Y1_ADR_HIGH,
MIN_Y1_ADR_LOW,
CNT_Y1_ADR_HIGH,
CNT_Y1_ADR_LOW,
MAX_X2_ADR_HIGH,
MAX_X2_ADR_LOW,
MIN_X2_ADR_HIGH,
MIN_X2_ADR_LOW,
CNT_X2_ADR_HIGH,
CNT_X2_ADR_LOW,
MAX_Y2_ADR_HIGH,
MAX_Y2_ADR_LOW,
MIN_Y2_ADR_HIGH,
MIN_Y2_ADR_LOW,
CNT_Y2_ADR_HIGH,
CNT_Y2_ADR_LOW,
EEPROM_ADR_NBR_OF_BYTES
};
int joystick_counter = 0;
int joystick_x1_12bit = 0;
int joystick_x1_12bit_max = 4096;
int joystick_x1_12bit_min = 0;
int joystick_x1_12bit_center = joystick_x1_12bit_max / 2;
int joystick_y1_12bit = 0;
int joystick_y1_12bit_max = 4096;
int joystick_y1_12bit_min = 0;
int joystick_y1_12bit_center = joystick_y1_12bit_max / 2;
int joystick_x2_12bit = 0;
int joystick_x2_12bit_max = 4096;
int joystick_x2_12bit_min = 0;
int joystick_x2_12bit_center = joystick_x2_12bit_max / 2;
int joystick_y2_12bit = 0;
int joystick_y2_12bit_max = 4096;
int joystick_y2_12bit_min = 0;
int joystick_y2_12bit_center = joystick_y2_12bit_max / 2;
float exp_constant = 0.2;
int fn_mode = 0;
ResponsiveAnalogRead analog_x1(A0, true);
ResponsiveAnalogRead analog_y1(A1, true);
ResponsiveAnalogRead analog_x2(A3, true);
ResponsiveAnalogRead analog_y2(A2, true);
ElrsTx elrs(Serial1);
const int CALIBRATION_OFF = 0;
const int CALIBRATION_INIT = 1;
const int CALIBRATION_CENTER = 2;
const int CALIBRATION_MINMAX = 3;
int joystick_calibration_mode = CALIBRATION_OFF;
/**
* @brief Save calibration data to EEPROM
*/
void save_to_eeprom() {
EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max);
EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_12bit_max >> 8);
EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_12bit_min);
EEPROM.write(MIN_X1_ADR_HIGH, joystick_x1_12bit_min >> 8);
EEPROM.write(CNT_X1_ADR_LOW, joystick_x1_12bit_center);
EEPROM.write(CNT_X1_ADR_HIGH, joystick_x1_12bit_center >> 8);
EEPROM.write(MAX_Y1_ADR_LOW, joystick_y1_12bit_max);
EEPROM.write(MAX_Y1_ADR_HIGH, joystick_y1_12bit_max >> 8);
EEPROM.write(MIN_Y1_ADR_LOW, joystick_y1_12bit_min);
EEPROM.write(MIN_Y1_ADR_HIGH, joystick_y1_12bit_min >> 8);
EEPROM.write(CNT_Y1_ADR_LOW, joystick_y1_12bit_center);
EEPROM.write(CNT_Y1_ADR_HIGH, joystick_y1_12bit_center >> 8);
EEPROM.write(MAX_X2_ADR_LOW, joystick_x2_12bit_max);
EEPROM.write(MAX_X2_ADR_HIGH, joystick_x2_12bit_max >> 8);
EEPROM.write(MIN_X2_ADR_LOW, joystick_x2_12bit_min);
EEPROM.write(MIN_X2_ADR_HIGH, joystick_x2_12bit_min >> 8);
EEPROM.write(CNT_X2_ADR_LOW, joystick_x2_12bit_center);
EEPROM.write(CNT_X2_ADR_HIGH, joystick_x2_12bit_center >> 8);
EEPROM.write(MAX_Y2_ADR_LOW, joystick_y2_12bit_max);
EEPROM.write(MAX_Y2_ADR_HIGH, joystick_y2_12bit_max >> 8);
EEPROM.write(MIN_Y2_ADR_LOW, joystick_y2_12bit_min);
EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_12bit_min >> 8);
EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_12bit_center);
EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_12bit_center >> 8);
}
/**
* @brief Load calibration data from EEPROM
*/
void load_from_eeprom() {
joystick_x1_12bit_max = (EEPROM.read(MAX_X1_ADR_HIGH) << 8);
joystick_x1_12bit_max |= EEPROM.read(MAX_X1_ADR_LOW);
joystick_x1_12bit_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8);
joystick_x1_12bit_min |= EEPROM.read(MIN_X1_ADR_LOW);
joystick_x1_12bit_center = (EEPROM.read(CNT_X1_ADR_HIGH) << 8);
joystick_x1_12bit_center |= EEPROM.read(CNT_X1_ADR_LOW);
joystick_y1_12bit_max = (EEPROM.read(MAX_Y1_ADR_HIGH) << 8);
joystick_y1_12bit_max |= EEPROM.read(MAX_Y1_ADR_LOW);
joystick_y1_12bit_min = (EEPROM.read(MIN_Y1_ADR_HIGH) << 8);
joystick_y1_12bit_min |= EEPROM.read(MIN_Y1_ADR_LOW);
joystick_y1_12bit_center = (EEPROM.read(CNT_Y1_ADR_HIGH) << 8);
joystick_y1_12bit_center |= EEPROM.read(CNT_Y1_ADR_LOW);
joystick_x2_12bit_max = (EEPROM.read(MAX_X2_ADR_HIGH) << 8);
joystick_x2_12bit_max |= EEPROM.read(MAX_X2_ADR_LOW);
joystick_x2_12bit_min = (EEPROM.read(MIN_X2_ADR_HIGH) << 8);
joystick_x2_12bit_min |= EEPROM.read(MIN_X2_ADR_LOW);
joystick_x2_12bit_center = (EEPROM.read(CNT_X2_ADR_HIGH) << 8);
joystick_x2_12bit_center |= EEPROM.read(CNT_X2_ADR_LOW);
joystick_y2_12bit_max = (EEPROM.read(MAX_Y2_ADR_HIGH) << 8);
joystick_y2_12bit_max |= EEPROM.read(MAX_Y2_ADR_LOW);
joystick_y2_12bit_min = (EEPROM.read(MIN_Y2_ADR_HIGH) << 8);
joystick_y2_12bit_min |= EEPROM.read(MIN_Y2_ADR_LOW);
joystick_y2_12bit_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8);
joystick_y2_12bit_center |= EEPROM.read(CNT_Y2_ADR_LOW);
}
/**
* @brief Calibrate all joystick axis
*
* @param analog_x1
* @param analog_y1
* @param analog_x2
* @param analog_y2
*/
void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) {
joystick_x1_12bit = AXIS_12BIT_CENTER;
joystick_y1_12bit = AXIS_12BIT_CENTER;
joystick_x2_12bit = AXIS_12BIT_CENTER;
joystick_y2_12bit = AXIS_12BIT_CENTER;
// Check for calibration mode
if (joystick_calibration_mode == CALIBRATION_INIT && digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED) {
joystick_calibration_mode = CALIBRATION_CENTER;
}
// Calibrate joystick center values
else if (joystick_calibration_mode == CALIBRATION_CENTER) {
joystick_x1_12bit_center = analog_x1;
joystick_y1_12bit_center = analog_y1;
joystick_x1_12bit_max = joystick_x1_12bit_center;
joystick_x1_12bit_min = joystick_x1_12bit_center;
joystick_y1_12bit_max = joystick_y1_12bit_center;
joystick_y1_12bit_min = joystick_y1_12bit_center;
joystick_x2_12bit_center = analog_x2;
joystick_y2_12bit_center = analog_y2;
joystick_x2_12bit_max = joystick_x2_12bit_center;
joystick_x2_12bit_min = joystick_x2_12bit_center;
joystick_y2_12bit_max = joystick_y2_12bit_center;
joystick_y2_12bit_min = joystick_y2_12bit_center;
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
joystick_calibration_mode = CALIBRATION_MINMAX;
}
}
// Calibrate joystick min/max values
else if (joystick_calibration_mode == CALIBRATION_MINMAX) {
if (analog_x1 > joystick_x1_12bit_max) joystick_x1_12bit_max = analog_x1;
if (analog_x1 < joystick_x1_12bit_min) joystick_x1_12bit_min = analog_x1;
if (analog_y1 > joystick_y1_12bit_max) joystick_y1_12bit_max = analog_y1;
if (analog_y1 < joystick_y1_12bit_min) joystick_y1_12bit_min = analog_y1;
if (analog_x2 > joystick_x2_12bit_max) joystick_x2_12bit_max = analog_x2;
if (analog_x2 < joystick_x2_12bit_min) joystick_x2_12bit_min = analog_x2;
if (analog_y2 > joystick_y2_12bit_max) joystick_y2_12bit_max = analog_y2;
if (analog_y2 < joystick_y2_12bit_min) joystick_y2_12bit_min = analog_y2;
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) {
joystick_calibration_mode = CALIBRATION_OFF;
save_to_eeprom();
}
}
}
/**
* @brief Save joystick calibration values to EEPROM
*/
void send_elrs_data() {
// Set ELRS analog channels
elrs.set_data(map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 0);
elrs.set_data(map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1);
elrs.set_data(map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 2);
elrs.set_data(map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3);
// Set ELRS digital channels
for (int i = 4; i < CRSF_MAX_CHANNEL; i++) {
elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i);
}
if (toggle_button_arm) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 4);
if (toggle_button_mode) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 5);
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 6);
if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 7);
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 8);
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 9);
// Send ELRS data
elrs.send_data();
}
/**
* Send USB data to PC
*/
void send_usb_data() {
// Set USB analog channels
int joystick_x1_10bit = map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
int joystick_y1_10bit = map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
int joystick_x2_10bit = map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
int joystick_y2_10bit = map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
Joystick.Zrotate(joystick_x1_10bit);
Joystick.Z(joystick_y1_10bit);
if (fn_mode == 2) {
Joystick.X(AXIS_10BIT_CENTER);
Joystick.Y(AXIS_10BIT_CENTER);
Joystick.sliderRight(joystick_x2_10bit);
Joystick.sliderLeft(joystick_y2_10bit);
} else if (fn_mode == 1) {
Joystick.X(AXIS_10BIT_CENTER);
Joystick.Y(joystick_y2_10bit);
Joystick.sliderRight(joystick_x2_10bit);
Joystick.sliderLeft(AXIS_10BIT_CENTER);
} else {
Joystick.X(joystick_x2_10bit);
Joystick.Y(joystick_y2_10bit);
Joystick.sliderRight(AXIS_10BIT_CENTER);
Joystick.sliderLeft(AXIS_10BIT_CENTER);
}
// Set USB digital channels
for (int i = 1; i < 32; i++) {
Joystick.button(i, 0);
}
Joystick.hat(JOYSTICK_HAT_CENTER);
if (fn_mode == 2) {
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1);
if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(13, 1);
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(14, 1);
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(15, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1);
} else if (fn_mode == 1) {
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1);
if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(9, 1);
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(10, 1);
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(11, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1);
} else {
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1);
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1);
if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1);
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1);
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1);
if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1);
}
Joystick.send_now();
}
/**
* @brief Apply calibration to 12-bit gimbal value
*
* @param gimbal_value
* @param min_value
* @param max_value
* @param center_value
* @param deadband_value
* @param expo_value
*/
int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int center_value, int deadband_value, int expo_value) {
int calibrated_value = AXIS_12BIT_CENTER;
if (gimbal_value > (center_value + deadband_value)) {
calibrated_value = constrain(map(gimbal_value, (center_value + deadband_value), max_value, AXIS_12BIT_CENTER, AXIS_12BIT_MAX),
AXIS_12BIT_CENTER, AXIS_12BIT_MAX);
} else if (gimbal_value < (center_value - deadband_value)) {
calibrated_value = constrain(map(gimbal_value, min_value, (center_value - deadband_value), AXIS_12BIT_MIN, AXIS_12BIT_CENTER),
AXIS_12BIT_MIN, AXIS_12BIT_CENTER);
}
if (expo_value != 0) {
float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX);
/* Calculate expo using 9th order polynomial function with 0.5 as center point */
float joystick_x_exp = expo_value * (0.5 + 256 * pow((joystick_x_float - 0.5), 9)) + (1 - expo_value) * joystick_x_float;
calibrated_value = constrain(int(joystick_x_exp * float(AXIS_12BIT_MAX)), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
}
return calibrated_value;
}
/**
* @brief Process input data from gimbal and buttons
*/
void process_input_data() {
int analog_x1_gimbal_value = 0;
int analog_y1_gimbal_value = 0;
int analog_x2_gimbal_value = 0;
int analog_y2_gimbal_value = 0;
if (gimbal_mode == GIMBAL_MODE_FRSKY_M10) {
analog_x1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
analog_y1_gimbal_value = constrain(analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
analog_x2_gimbal_value = constrain(analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
analog_y2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
} else if (gimbal_mode == GIMBAL_MODE_FRSKY_M7) {
analog_x1_gimbal_value = constrain(analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
analog_y1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
analog_x2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
analog_y2_gimbal_value = constrain(analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
}
if (joystick_calibration_mode != CALIBRATION_OFF) {
calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value);
return;
}
joystick_x1_12bit = apply_calibration_12bit(analog_x1_gimbal_value, joystick_x1_12bit_min, joystick_x1_12bit_max,
joystick_x1_12bit_center, DEADZONE_X, exp_constant);
joystick_y1_12bit = apply_calibration_12bit(analog_y1_gimbal_value, joystick_y1_12bit_min, joystick_y1_12bit_max,
joystick_y1_12bit_center, DEADZONE_Y, 0);
joystick_x2_12bit = apply_calibration_12bit(analog_x2_gimbal_value, joystick_x2_12bit_min, joystick_x2_12bit_max,
joystick_x2_12bit_center, DEADZONE_X, exp_constant);
joystick_y2_12bit = apply_calibration_12bit(analog_y2_gimbal_value, joystick_y2_12bit_min, joystick_y2_12bit_max,
joystick_y2_12bit_center, DEADZONE_Y, exp_constant);
// Check fn mode
fn_mode = 0;
if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
fn_mode = 1;
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) {
fn_mode = 2;
}
}
// Check toggle mode buttons
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) {
toggle_button_mode = true;
} else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
toggle_button_mode = false;
}
// Check toggle arm button
if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) {
toggle_button_arm = true;
} else if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) {
toggle_button_arm = false;
}
}
void setup() {
/* Init HW */
status_led.begin();
status_led.blink();
button_led_1.begin();
button_led_2.begin();
pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP);
pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP);
pinMode(BUTTON_FRONT_RIGHT_LOWER_PIN, INPUT_PULLUP);
pinMode(BUTTON_FRONT_RIGHT_UPPER_PIN, INPUT_PULLUP);
pinMode(BUTTON_TOP_LEFT_LOWER_PIN, INPUT_PULLUP);
pinMode(BUTTON_TOP_RIGHT_LOWER_PIN, INPUT_PULLUP);
pinMode(BUTTON_TOP_LEFT_UPPER_PIN, INPUT_PULLUP);
pinMode(BUTTON_TOP_RIGHT_UPPER_PIN, INPUT_PULLUP);
pinMode(GIMBAL_MODE_PIN, INPUT_PULLUP);
// Set ADC resolution to 12bit
analogReadResolution(12);
analogReadAveraging(32);
delay(500);
// Set analog (lib) resolution to 12bit
analog_x1.setAnalogResolution(4096);
analog_y1.setAnalogResolution(4096);
analog_x2.setAnalogResolution(4096);
analog_y2.setAnalogResolution(4096);
// Init EEPROM
load_from_eeprom();
// Init Joystick
Joystick.useManualSend(true);
// Check if calibration mode is enabled
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
joystick_calibration_mode = CALIBRATION_INIT;
}
// Check if bootloader mode is enabled
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) {
button_led_2.on();
button_led_2.update();
delay(200);
_reboot_Teensyduino_();
}
// Check what gimbal mode is selected
if (digitalRead(GIMBAL_MODE_PIN) == LOW) {
gimbal_mode = GIMBAL_MODE_FRSKY_M10;
}
// Init ELRS
elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW);
}
void loop() {
current_timestamp_micros = micros();
analog_x1.update();
analog_y1.update();
analog_x2.update();
analog_y2.update();
/* Process data with 1ms interval*/
if (current_timestamp_micros >= process_data_timestamp_micros) {
process_input_data();
process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms;
}
/* Update/Send USB data with 5ms interval*/
if (current_timestamp_micros >= send_usb_timestamp_micros) {
send_usb_data();
send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms;
}
/* Update/Send ERLS data with about 1,6ms interval */
if (current_timestamp_micros >= send_elrs_timestamp_micros) {
send_elrs_data();
send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
}
/* Update indicator with 200ms interval */
if (current_timestamp_micros >= indicator_timestamp_micros) {
button_led_1.off();
button_led_2.off();
if (joystick_calibration_mode == CALIBRATION_INIT) {
button_led_1.blink();
button_led_2.blink();
} else if (joystick_calibration_mode == CALIBRATION_CENTER) {
button_led_1.blink();
button_led_2.off();
} else if (joystick_calibration_mode == CALIBRATION_MINMAX) {
button_led_1.off();
button_led_2.blink();
} else if ((joystick_x1_12bit != AXIS_12BIT_CENTER) || (joystick_y1_12bit != AXIS_12BIT_MIN) ||
(joystick_x2_12bit != AXIS_12BIT_CENTER) || (joystick_y2_12bit != AXIS_12BIT_CENTER)) {
button_led_1.on();
button_led_2.on();
}
status_led.update();
button_led_1.update();
button_led_2.update();
indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms;
}
}

863
firmware/src/main.cpp
View File

@ -1,863 +0,0 @@
/*
* =======================================================================================================
* -------------------------------------------------------------------------------------------------------
* ---####################-----###########-------###########-----############--############-############--
* --######################---#############-----#############---- -- - ---
* --###### ##---##### ###-----### #####---------##-------#######------#-------------
* -- -------------- --- ----- --- ----- ---------##-------#------------#-------------
* --#####--------------------#####------####-####------#####---------##-------###########--############--
* -- -------------------- ------ ------ --------- ------- -- --
* --#####--------------------#####--------#####--------#####---------------------------------------------
* -- -------------------- -------- -------- ---------------------------------------------
* --######--------------##---#####---------------------#####---------- CMtec CMDR Joystick RC -----------
* --##################### ---#####---------------------#####---------------------------------------------
* ---################### ----#####---------------------#####---------------------------------------------
* --- ----- --------------------- ---------------------------------------------
* -------------------------------------------------------------------------------------------------------
* =======================================================================================================
*
* Copyright 2022 Christoffer Martinsson <cm@cmtec.se>
*
* CMtec CMDR Joystick RC can be redistributed and/or modified under the terms of the GNU General
* Public License (Version 2), as published by the Free Software Foundation.
* A copy of the license can be found online at www.gnu.o urg/licenses.
*
* CMtec CMDR Joystick RC 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.
*
* Joystick based on standard teensy "Keypad" library for button scanning, standard teensy
* "usb_joystick" library for HID joystick usb data communication.
*
* Layer 0
*
* | B7 | | B9 |
* ---------------------------------------------
* | B8 | | | B1 | B2 | | | B10 |
* | X1,Y1 | | B3 | B4 | | X2,Y2 |
* | Fn1 | | B5 |Y1 offset| B6 | | Fn2 |
* ---------------------------------------------
*
* Layer 1 (Fn1)
*
* | B17| | B19|
* ---------------------------------------------
* | B18 | | | B11| B12| | | B20 |
* | X1,Y1 | | B13| B14| | X3,Y2 |
* | Fn1 | | B15|Y1 offset| B16| | Fn2 |
* ---------------------------------------------
*
* Layer 2 (Fn2)
*
* | B27| | B29|
* ---------------------------------------------
* | B28 | | | B21| B22| | | B30 |
* | X1,Y1 | | B23| B24| | X3,Y3 |
* | Fn1 | | B25|Y1 offset| B26| | Fn2 |
* ---------------------------------------------
*
* Features:
*
* * 5 physical axis.
* * 6 joystick axis using layers.
* * 12 physical buttons.
* * 32 joystick buttons using three layers (Fn1 and Fn2).
*/
#include <Arduino.h>
#include <Keypad.h>
#include <EEPROM.h>
#define USB_LED_NUM_LOCK 0
#define USB_LED_CAPS_LOCK 1
#define USB_LED_SCROLL_LOCK 2
#define KEY_OFFSET 0xAA00 // Offset to apply for not interfere with already defined keyboard keys
#define NO_JOY 0
#define JOY_A1 1 + KEY_OFFSET
#define JOY_A2 2 + KEY_OFFSET
#define JOY_A3 3 + KEY_OFFSET
#define JOY_A4 4 + KEY_OFFSET
#define JOY_A5 5 + KEY_OFFSET
#define JOY_A6 6 + KEY_OFFSET
#define JOY_A7 7 + KEY_OFFSET
#define JOY_A8 8 + KEY_OFFSET
#define JOY_A9 9 + KEY_OFFSET
#define JOY_A10 10 + KEY_OFFSET
#define JOY_A11 11 + KEY_OFFSET
#define JOY_A12 12 + KEY_OFFSET
#define JOY_A13 13 + KEY_OFFSET
#define JOY_A14 14 + KEY_OFFSET
#define JOY_A15 15 + KEY_OFFSET
#define JOY_A16 16 + KEY_OFFSET
#define JOY_A17 17 + KEY_OFFSET
#define JOY_A18 18 + KEY_OFFSET
#define JOY_A19 19 + KEY_OFFSET
#define JOY_A20 20 + KEY_OFFSET
#define JOY_A21 21 + KEY_OFFSET
#define JOY_A22 22 + KEY_OFFSET
#define JOY_A23 23 + KEY_OFFSET
#define JOY_A24 24 + KEY_OFFSET
#define JOY_A25 25 + KEY_OFFSET
#define JOY_A26 26 + KEY_OFFSET
#define JOY_A27 27 + KEY_OFFSET
#define JOY_A28 28 + KEY_OFFSET
#define JOY_A29 29 + KEY_OFFSET
#define JOY_A30 30 + KEY_OFFSET
#define JOY_A31 31 + KEY_OFFSET
#define JOY_A32 32 + KEY_OFFSET
#define JOY_AHU1 49 + KEY_OFFSET
#define JOY_AHR1 50 + KEY_OFFSET
#define JOY_AHD1 51 + KEY_OFFSET
#define JOY_AHL1 52 + KEY_OFFSET
#define KEY_FN1 61 + KEY_OFFSET // Function layer 1 button
#define KEY_FN2 62 + KEY_OFFSET // Function layer 2 button
#define TAP_TIMEOUT 150 // Key tap timeout (ms)
#define NBR_OF_BUTTONS 12 // Number of buttons used (12 in this case)
#define HID_AXIS_MAX 1023
#define HID_AXIS_MIN 0
#define HID_AXIS_CENTER 512
#define AXIS_MAX 4096
#define AXIS_MIN 0
#define AXIS_CENTER 2048
#define DEADZONE_X 50
#define DEADZONE_Y 50
struct Button
{
int keypad_kchar = 0;
uint16_t keycode = NO_KEY;
uint16_t tap_keycode = NO_KEY;
uint16_t fn1_keycode = NO_KEY;
uint16_t fn2_keycode = NO_KEY;
bool hold_direct = true;
int kstate = IDLE;
uint16_t last_keycode = NO_KEY;
bool run_keycode = false;
int tap_state = 0;
bool tap_timeout_enable = false;
bool tap_release_enable = false;
unsigned long tap_timeout_timestamp = 0;
unsigned long tap_release_timestamp = 0;
bool tap_inhibit = false;
};
const byte KP_ROWS = 3;
const byte KP_COLS = 4;
byte kp_rowPins[KP_ROWS] = {6, 7, 8};
byte kp_colPins[KP_COLS] = {9, 10, 11, 12};
char kp_keys[KP_ROWS][KP_COLS] = {
{1, 2, 3, 4},
{5, 6, 7, 8},
{9, 10, 11, 12}};
Keypad kp_keypad = Keypad(makeKeymap(kp_keys), kp_rowPins, kp_colPins, KP_ROWS, KP_COLS);
/* Valid "Fn0 (hold) key" when using tap mode are: KEY_LEFT_SHIFT, KEY_RIGHT_SHIFT, KEY_LEFT_CTRL, KEY_RIGHT_CTRL, KEY_RIGHT_ALT, KEY_LEFT_GUI, KEY_RIGHT_GUI, KEY_FN1, KEY_FN2
* "Button ID" corresponding with the physical design of the actual keyboard. DO NOT CHANGE BTN ID!
| 1 | | 4 |
---------------------------------------------
| 5 | | | 2 | 3 | | | 8 |
| X1,Y1 | | 6 | 7 | | X3,Y3 |
| 9 | | 10 |Y1 offset| 11 | | 12 |
---------------------------------------------
* "Fn0 hold key" is normal key in non tap mode. In tap mode this key is the hold key.
* "Fn0 tap key" enables tap mode.
* "Fn1 key" is the layer 1 key to use. Don NOT add KEY_FN1 or KEY_FN2 to this layer.
* "Fn2 key" is the layer 2 key to use. Don NOT add KEY_FN1 or KEY_FN2 to this layer.
"Fn1 key" and "Fn2 key" are N/A when using tap mode and should me defined as NO_KEY.
* "Hold direct" enables sending PRESS command as soon as the hold key is pressed (regardless if you intend to press the tap key).
/* Keymap config ----------------------------------------------------------------------------------------------------------------------------------- */
// clang-format off
Button buttons[NBR_OF_BUTTONS] =
{
/* Btn ID Fn0 (hold) key Fn1 key Fn2 key */
{1, JOY_A7, JOY_A17, JOY_A27, IDLE, NO_KEY, false},
{2, JOY_A1, JOY_A11, JOY_A21, IDLE, NO_KEY, false},
{3, JOY_A2, JOY_A12, JOY_A22, IDLE, NO_KEY, false},
{4, JOY_A9, JOY_A19, JOY_A29, IDLE, NO_KEY, false},
{5, JOY_A8, JOY_A18, JOY_A28, IDLE, NO_KEY, false},
{6, JOY_A3, JOY_A13, JOY_A23, IDLE, NO_KEY, false},
{7, JOY_A4, JOY_A14, JOY_A24, IDLE, NO_KEY, false},
{8, JOY_A10, JOY_A20, JOY_A30, IDLE, NO_KEY, false},
{9, KEY_FN1, NO_KEY, NO_KEY, IDLE, NO_KEY, false},
{10, JOY_A5, JOY_A15, JOY_A25, IDLE, NO_KEY, false},
{11, JOY_A6, JOY_A16, JOY_A26, IDLE, NO_KEY, false},
{12, KEY_FN2, NO_KEY, NO_KEY, IDLE, NO_KEY, false}};
// clang-format on
/* End of keymap config ----------------------------------------------------------------------------------------------------------------------------- */
const int STATUS_LED = 13;
bool status_led_on = false;
int status_led_mode = 0;
unsigned long current_timestamp = 0;
unsigned long button_timestamp = 0;
unsigned long indicator_timestamp = 0;
bool key_pressed = false;
int fn_mode = 0;
// USB HID data variables
int xy_x1 = HID_AXIS_CENTER;
int xy_y1 = HID_AXIS_CENTER;
int xy_x2 = HID_AXIS_CENTER;
int xy_y2 = HID_AXIS_CENTER;
int xy_x3 = HID_AXIS_CENTER;
int xy_y3 = HID_AXIS_CENTER;
enum EEPROM_ADR
{
MAX_X1_ADR_HIGH,
MAX_X1_ADR_LOW,
MIN_X1_ADR_HIGH,
MIN_X1_ADR_LOW,
CNT_X1_ADR_HIGH,
CNT_X1_ADR_LOW,
MAX_Y1_ADR_HIGH,
MAX_Y1_ADR_LOW,
MIN_Y1_ADR_HIGH,
MIN_Y1_ADR_LOW,
CNT_Y1_ADR_HIGH,
CNT_Y1_ADR_LOW,
MAX_X2_ADR_HIGH,
MAX_X2_ADR_LOW,
MIN_X2_ADR_HIGH,
MIN_X2_ADR_LOW,
CNT_X2_ADR_HIGH,
CNT_X2_ADR_LOW,
MAX_Y2_ADR_HIGH,
MAX_Y2_ADR_LOW,
MIN_Y2_ADR_HIGH,
MIN_Y2_ADR_LOW,
CNT_Y2_ADR_HIGH,
CNT_Y2_ADR_LOW,
MAX_X3_ADR_HIGH,
MAX_X3_ADR_LOW,
MIN_X3_ADR_HIGH,
MIN_X3_ADR_LOW,
CNT_X3_ADR_HIGH,
CNT_X3_ADR_LOW,
EEPROM_ADR_NBR_OF_BYTES
};
int joystick_counter = 0;
int joystick_x1 = 0;
int joystick_x1_raw = 0;
int joystick_x1_max = 4096;
int joystick_x1_min = 0;
int joystick_x1_center = joystick_x1_max/2;
int joystick_y1 = 0;
int joystick_y1_raw = 0;
int joystick_y1_max = 4096;
int joystick_y1_min = 0;
int joystick_y1_center = joystick_y1_max/2;
int joystick_x2 = 0;
int joystick_x2_raw = 0;
int joystick_x2_max = 4096;
int joystick_x2_min = 0;
int joystick_x2_center = joystick_x2_max/2;
int joystick_y2 = 0;
int joystick_y2_raw = 0;
int joystick_y2_max = 4096;
int joystick_y2_min = 0;
int joystick_y2_center = joystick_y2_max/2;
int joystick_x3 = 0;
int joystick_x3_raw = 0;
int joystick_x3_max = 4096;
int joystick_x3_min = 0;
int joystick_x3_center = joystick_x3_max/2;
float exp_constant = 0.2;
#define CALIBRATION_OFF 0
#define CALIBRATION_CENTER 1
#define CALIBRATION_MINMAX 2
int joystick_calibration_mode = 0;
void save_to_eeprom(){
EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_max);
EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_max >> 8);
EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_min);
EEPROM.write(MIN_X1_ADR_HIGH, joystick_x1_min >> 8);
EEPROM.write(CNT_X1_ADR_LOW, joystick_x1_center);
EEPROM.write(CNT_X1_ADR_HIGH, joystick_x1_center >> 8);
EEPROM.write(MAX_Y1_ADR_LOW, joystick_y1_max);
EEPROM.write(MAX_Y1_ADR_HIGH, joystick_y1_max >> 8);
EEPROM.write(MIN_Y1_ADR_LOW, joystick_y1_min);
EEPROM.write(MIN_Y1_ADR_HIGH, joystick_y1_min >> 8);
EEPROM.write(CNT_Y1_ADR_LOW, joystick_y1_center);
EEPROM.write(CNT_Y1_ADR_HIGH, joystick_y1_center >> 8);
EEPROM.write(MAX_X2_ADR_LOW, joystick_x2_max);
EEPROM.write(MAX_X2_ADR_HIGH, joystick_x2_max >> 8);
EEPROM.write(MIN_X2_ADR_LOW, joystick_x2_min);
EEPROM.write(MIN_X2_ADR_HIGH, joystick_x2_min >> 8);
EEPROM.write(CNT_X2_ADR_LOW, joystick_x2_center);
EEPROM.write(CNT_X2_ADR_HIGH, joystick_x2_center >> 8);
EEPROM.write(MAX_Y2_ADR_LOW, joystick_y2_max);
EEPROM.write(MAX_Y2_ADR_HIGH, joystick_y2_max >> 8);
EEPROM.write(MIN_Y2_ADR_LOW, joystick_y2_min);
EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_min >> 8);
EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_center);
EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_center >> 8);
EEPROM.write(MAX_X3_ADR_LOW, joystick_x3_max);
EEPROM.write(MAX_X3_ADR_HIGH, joystick_x3_max >> 8);
EEPROM.write(MIN_X3_ADR_LOW, joystick_x3_min);
EEPROM.write(MIN_X3_ADR_HIGH, joystick_x3_min >> 8);
EEPROM.write(CNT_X3_ADR_LOW, joystick_x3_center);
EEPROM.write(CNT_X3_ADR_HIGH, joystick_x3_center >> 8);
}
void load_from_eeprom(){
joystick_x1_max = (EEPROM.read(MAX_X1_ADR_HIGH) << 8);
joystick_x1_max |= EEPROM.read(MAX_X1_ADR_LOW);
joystick_x1_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8);
joystick_x1_min |= EEPROM.read(MIN_X1_ADR_LOW);
joystick_x1_center = (EEPROM.read(CNT_X1_ADR_HIGH) << 8);
joystick_x1_center |= EEPROM.read(CNT_X1_ADR_LOW);
joystick_y1_max = (EEPROM.read(MAX_Y1_ADR_HIGH) << 8);
joystick_y1_max |= EEPROM.read(MAX_Y1_ADR_LOW);
joystick_y1_min = (EEPROM.read(MIN_Y1_ADR_HIGH) << 8);
joystick_y1_min |= EEPROM.read(MIN_Y1_ADR_LOW);
joystick_y1_center = (EEPROM.read(CNT_Y1_ADR_HIGH) << 8);
joystick_y1_center |= EEPROM.read(CNT_Y1_ADR_LOW);
joystick_x2_max = (EEPROM.read(MAX_X2_ADR_HIGH) << 8);
joystick_x2_max |= EEPROM.read(MAX_X2_ADR_LOW);
joystick_x2_min = (EEPROM.read(MIN_X2_ADR_HIGH) << 8);
joystick_x2_min |= EEPROM.read(MIN_X2_ADR_LOW);
joystick_x2_center = (EEPROM.read(CNT_X2_ADR_HIGH) << 8);
joystick_x2_center |= EEPROM.read(CNT_X2_ADR_LOW);
joystick_y2_max = (EEPROM.read(MAX_Y2_ADR_HIGH) << 8);
joystick_y2_max |= EEPROM.read(MAX_Y2_ADR_LOW);
joystick_y2_min = (EEPROM.read(MIN_Y2_ADR_HIGH) << 8);
joystick_y2_min |= EEPROM.read(MIN_Y2_ADR_LOW);
joystick_y2_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8);
joystick_y2_center |= EEPROM.read(CNT_Y2_ADR_LOW);
joystick_x3_max = (EEPROM.read(MAX_X3_ADR_HIGH) << 8);
joystick_x3_max |= EEPROM.read(MAX_X3_ADR_LOW);
joystick_x3_min = (EEPROM.read(MIN_X3_ADR_HIGH) << 8);
joystick_x3_min |= EEPROM.read(MIN_X3_ADR_LOW);
joystick_x3_center = (EEPROM.read(CNT_X3_ADR_HIGH) << 8);
joystick_x3_center |= EEPROM.read(CNT_X2_ADR_LOW);
}
void update_analog(){
if(joystick_counter == 0){
joystick_x1_raw = analogRead(0);
joystick_counter++;
}
else if(joystick_counter == 1){
joystick_y1_raw = analogRead(1);
joystick_counter++;
}
else if(joystick_counter == 2){
joystick_x2_raw = analogRead(2);
joystick_counter++;
}
else if(joystick_counter == 3){
joystick_y2_raw = analogRead(3);
joystick_counter++;
}
else if(joystick_counter == 4){
joystick_x3_raw = analogRead(4);
joystick_counter = 0;
if (joystick_calibration_mode == CALIBRATION_OFF){
// ----------------------------------------------------------
// Map X1 joystick values to proper HID values
// ----------------------------------------------------------
if(joystick_x1_raw > (joystick_x1_center + DEADZONE_X)){
joystick_x1 = constrain(map(joystick_x1_raw, (joystick_x1_center + DEADZONE_X), joystick_x1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
}
else if(joystick_x1_raw < (joystick_x1_center - DEADZONE_X)){
joystick_x1 = constrain(map(joystick_x1_raw, joystick_x1_min, (joystick_x1_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
}
else{
joystick_x1 = AXIS_CENTER;
}
// ----------------------------------------------------------
// Map Y1 joystick values to proper HID values
// ----------------------------------------------------------
if(joystick_y1_raw > (joystick_y1_center + DEADZONE_Y)){
joystick_y1 = constrain(map(joystick_y1_raw, (joystick_y1_center + DEADZONE_Y), joystick_y1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
}
else if(joystick_y1_raw < (joystick_y1_center - DEADZONE_Y)){
joystick_y1 = constrain(map(joystick_y1_raw, joystick_y1_min, (joystick_y1_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
}
else{
joystick_y1 = AXIS_CENTER;
}
// ----------------------------------------------------------
// Map X2 joystick values to proper HID values
// ----------------------------------------------------------
if(joystick_x2_raw > (joystick_x2_center + DEADZONE_X)){
joystick_x2 = constrain(map(joystick_x2_raw, (joystick_x2_center + DEADZONE_X), joystick_x2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
}
else if(joystick_x2_raw < (joystick_x2_center - DEADZONE_X)){
joystick_x2 = constrain(map(joystick_x2_raw, joystick_x2_min, (joystick_x2_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
}
else{
joystick_x2 = AXIS_CENTER;
}
// ----------------------------------------------------------
// Map Y1 joystick values to proper HID values
// ----------------------------------------------------------
if(joystick_y2_raw > (joystick_y2_center + DEADZONE_Y)){
joystick_y2 = constrain(map(joystick_y2_raw, (joystick_y2_center + DEADZONE_Y), joystick_y2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
}
else if(joystick_y2_raw < (joystick_y2_center - DEADZONE_Y)){
joystick_y2 = constrain(map(joystick_y2_raw, joystick_y2_min, (joystick_y2_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
}
else{
joystick_y2 = AXIS_CENTER;
}
// ----------------------------------------------------------
// Map X3 joystick values to proper HID values
// ----------------------------------------------------------
if(joystick_x3_raw > (joystick_x3_center + DEADZONE_X)){
joystick_x3 = constrain(map(joystick_x3_raw, (joystick_x3_center + DEADZONE_X), joystick_x3_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX);
}
else if(joystick_x3_raw < (joystick_x3_center - DEADZONE_X)){
joystick_x3 = constrain(map(joystick_x3_raw, joystick_x3_min, (joystick_x3_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER);
}
else{
joystick_x3 = AXIS_CENTER;
}
// ----------------------------------------------------------
// Calculate new axis values after applying exp curve
// ----------------------------------------------------------
// Normal mode
exp_constant = 0.2;
if (joystick_x1 != AXIS_CENTER){
float joystick_x_float = joystick_x1 / float(AXIS_MAX);
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
joystick_x1 = int(joystick_x_exp * float(HID_AXIS_MAX));
joystick_x1 = constrain(joystick_x1, HID_AXIS_MIN, HID_AXIS_MAX);
}
else{
joystick_x1 = HID_AXIS_CENTER;
}
if (joystick_y1 != AXIS_CENTER){
float joystick_y_float = joystick_y1 / float(AXIS_MAX);
float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
joystick_y1 = int(joystick_y_exp * float(HID_AXIS_MAX));
joystick_y1 = constrain(joystick_y1, HID_AXIS_MIN, HID_AXIS_MAX);
}
else{
joystick_y1 = HID_AXIS_CENTER;
}
if (joystick_x2 != AXIS_CENTER){
float joystick_x_float = joystick_x2 / float(AXIS_MAX);
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
joystick_x2 = int(joystick_x_exp * float(HID_AXIS_MAX));
joystick_x2 = constrain(joystick_x2, HID_AXIS_MIN, HID_AXIS_MAX);
}
else{
joystick_x2 = HID_AXIS_CENTER;
}
if (joystick_y2 != AXIS_CENTER){
float joystick_y_float = joystick_y2 / float(AXIS_MAX);
float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float;
joystick_y2 = int(joystick_y_exp * float(HID_AXIS_MAX));
joystick_y2 = constrain(joystick_y2, HID_AXIS_MIN, HID_AXIS_MAX);
}
else{
joystick_y2 = HID_AXIS_CENTER;
}
if (joystick_x3 != AXIS_CENTER){
float joystick_x_float = joystick_x3 / float(AXIS_MAX);
float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float;
joystick_x3 = int(joystick_x_exp * float(HID_AXIS_MAX));
joystick_x3 = constrain(joystick_x3, HID_AXIS_MIN, HID_AXIS_MAX);
}
else{
joystick_x3 = HID_AXIS_CENTER;
}
}
else{
// ----------------------------------------------------------
// Calibration mode.
// ----------------------------------------------------------
joystick_x1 = HID_AXIS_CENTER;
joystick_x1 = HID_AXIS_CENTER;
joystick_x2 = HID_AXIS_CENTER;
joystick_y2 = HID_AXIS_CENTER;
joystick_x3 = HID_AXIS_CENTER;
if (joystick_calibration_mode == CALIBRATION_CENTER){
joystick_x1_center = joystick_x1_raw;
joystick_y1_center = joystick_y1_raw;
joystick_x1_max = joystick_x1_center;
joystick_x1_min = joystick_x1_center;
joystick_y1_max = joystick_y1_center;
joystick_y1_min = joystick_y1_center;
joystick_x2_center = joystick_x2_raw;
joystick_y2_center = joystick_y2_raw;
joystick_x2_max = joystick_x2_center;
joystick_x2_min = joystick_x2_center;
joystick_y2_max = joystick_y2_center;
joystick_y2_min = joystick_y2_center;
joystick_x3_center = joystick_x3_raw;
joystick_x3_max = joystick_x3_center;
joystick_x3_min = joystick_x3_center;
}
else if (joystick_calibration_mode == CALIBRATION_MINMAX){
if(joystick_x1_raw > joystick_x1_max){
joystick_x1_max = joystick_x1_raw;
}
if(joystick_x1_raw < joystick_x1_min){
joystick_x1_min = joystick_x1_raw;
}
if(joystick_y1_raw > joystick_y1_max){
joystick_y1_max = joystick_y1_raw;
}
if(joystick_y1_raw < joystick_y1_min){
joystick_y1_min = joystick_y1_raw;
}
if(joystick_x2_raw > joystick_x2_max){
joystick_x2_max = joystick_x2_raw;
}
if(joystick_x2_raw < joystick_x2_min){
joystick_x2_min = joystick_x2_raw;
}
if(joystick_y2_raw > joystick_y2_max){
joystick_y2_max = joystick_y2_raw;
}
if(joystick_y2_raw < joystick_y2_min){
joystick_y2_min = joystick_y2_raw;
}
if(joystick_x3_raw > joystick_x3_max){
joystick_x3_max = joystick_x3_raw;
}
if(joystick_x3_raw < joystick_x3_min){
joystick_x3_min = joystick_x3_raw;
}
}
}
}
}
/**
Perform key action.
@param keycode code to apply action.
@param kstate PRESSED or RELEASED.
@return Action applied.
*/
bool set_key(uint16_t keycode, uint8_t kstate)
{
/* Abort if keycode is invalid */
if (keycode == NO_KEY || keycode == KEY_FN1 || keycode == KEY_FN2)
{
return false;
}
/* Joystick buttons */
else if (keycode >= JOY_A1 && keycode <= JOY_A32) {
if (kstate == RELEASED) {
Joystick.button(keycode-KEY_OFFSET, false);
} else if (kstate == PRESSED) {
Joystick.button(keycode-KEY_OFFSET, true);
}
}
/* Joystick hat */
else if (keycode >= JOY_AHU1 && keycode <= JOY_AHL1) {
if (kstate == RELEASED) {
Joystick.hat(-1);
} else if (kstate == PRESSED) {
Joystick.hat(((keycode-KEY_OFFSET)-(JOY_AHU1-KEY_OFFSET)) * 90);
}
}
return true;
}
/**
Scan key matrix and perform processing for each key.
@return void.
*/
void scan_buttons()
{
/* Scan keypad */
if (kp_keypad.getKeys())
{
/* Enter bootloader if all reboot-buttons is pressed together */
int reboot = 0;
for (int i = 0; i < LIST_MAX; i++)
{
if ((kp_keypad.key[i].kchar == 1) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
reboot += 1;
}
if ((kp_keypad.key[i].kchar == 4) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
reboot += 1;
}
if ((kp_keypad.key[i].kchar == 10) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
reboot += 1;
}
if ((kp_keypad.key[i].kchar == 11) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
reboot += 1;
}
}
if (reboot == 4)
{
_reboot_Teensyduino_();
}
/* Enter calibration mode if all calibration-buttons is pressed together */
int calibrate = 0;
for (int i = 0; i < LIST_MAX; i++)
{
if ((kp_keypad.key[i].kchar == 2) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
calibrate += 1;
}
if ((kp_keypad.key[i].kchar == 3) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
calibrate += 1;
}
if ((kp_keypad.key[i].kchar == 10) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
calibrate += 1;
}
if ((kp_keypad.key[i].kchar == 11) && (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD))
{
calibrate += 1;
}
}
if (calibrate == 4)
{
joystick_calibration_mode = CALIBRATION_CENTER;
}
/* Check for Fn mode */
fn_mode = 0;
for (int i = 0; i < LIST_MAX; i++)
{
if (kp_keypad.key[i].kstate == PRESSED || kp_keypad.key[i].kstate == HOLD)
{
for (int j = 0; j < NBR_OF_BUTTONS; j++)
{
if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar)
{
/* Check if FN1 key are defined to this button (Layer 0 and first position in combo array)*/
if (buttons[j].keycode == KEY_FN1)
{
fn_mode++;
}
break;
}
}
}
}
/* Process key press/release */
for (int i = 0; i < LIST_MAX; i++)
{
if (kp_keypad.key[i].kstate == PRESSED)
{
for (int j = 0; j < NBR_OF_BUTTONS; j++)
{
if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar && kp_keypad.key[i].stateChanged == true)
{
buttons[j].run_keycode = true;
buttons[j].kstate = PRESSED;
break;
}
}
}
else if (kp_keypad.key[i].kstate == RELEASED)
{
for (int j = 0; j < NBR_OF_BUTTONS; j++)
{
if (buttons[j].keypad_kchar == kp_keypad.key[i].kchar && kp_keypad.key[i].stateChanged == true)
{
buttons[j].run_keycode = true;
buttons[j].kstate = RELEASED;
break;
}
}
}
}
/* Check if any "non tap keys" has been pressed */
for (int i = 0; i < NBR_OF_BUTTONS; i++)
{
if (buttons[i].run_keycode == true && buttons[i].tap_keycode == NO_KEY && buttons[i].kstate == PRESSED)
{
for (int j = 0; j < NBR_OF_BUTTONS; j++)
{
if (buttons[j].tap_keycode != NO_KEY)
{
buttons[j].tap_inhibit = true;
}
}
}
}
/* Execute key commands */
for (int i = 0; i < NBR_OF_BUTTONS; i++)
{
/* Check if key should be processed */
if (buttons[i].run_keycode == true)
{
/* Check if key pressed or released */
if (buttons[i].kstate == PRESSED)
{
if (fn_mode == 0)
{
set_key(buttons[i].keycode, PRESSED);
buttons[i].last_keycode = buttons[i].keycode;
}
else if (fn_mode == 1)
{
set_key(buttons[i].fn1_keycode, PRESSED);
buttons[i].last_keycode = buttons[i].fn1_keycode;
}
else if (fn_mode == 2)
{
set_key(buttons[i].fn2_keycode, PRESSED);
buttons[i].last_keycode = buttons[i].fn2_keycode;
}
}
else if (buttons[i].kstate == RELEASED)
{
/* Sending release command for last keycode related to this button */
set_key(buttons[i].last_keycode, RELEASED);
}
/* Reset run_keycode flag */
buttons[i].run_keycode = false;
}
}
}
}
void setup()
{
/* Init HW */
pinMode(STATUS_LED, OUTPUT);
digitalWrite(STATUS_LED, LOW);
// Set ADC resolution to 12bit
analogReadResolution(12);
analogReadAveraging(32);
delay(500);
// Init Joystick
Joystick.useManualSend(true);
}
void loop()
{
/* Update current time (ms) */
current_timestamp = millis();
/* Scan buttons 1ms */
if (current_timestamp >= button_timestamp)
{
button_timestamp = current_timestamp + 100;
update_analog();
//scan_buttons();
// ----------------------------------------------------------
// Update joystick axis data
// ----------------------------------------------------------
Joystick.X(joystick_x1);
Joystick.Y(joystick_y1);
if (fn_mode == 2){
Joystick.Z(HID_AXIS_CENTER);
Joystick.Zrotate(HID_AXIS_CENTER);
Joystick.sliderLeft(joystick_x2);
Joystick.sliderRight(joystick_y2);
}
else if (fn_mode == 1){
Joystick.Z(HID_AXIS_CENTER);
Joystick.Zrotate(joystick_y2);
Joystick.sliderLeft(joystick_x2);
Joystick.sliderRight(HID_AXIS_CENTER);
}
else{
Joystick.Z(joystick_x2);
Joystick.Zrotate(joystick_y2);
Joystick.sliderLeft(joystick_x3);
Joystick.sliderRight(joystick_x3);
}
Joystick.send_now();
}
/* Update indicator 200ms */
if (current_timestamp >= indicator_timestamp)
{
if (status_led_mode == 2 && status_led_on == false)
{
digitalWrite(STATUS_LED, HIGH);
status_led_on = true;
}
else if (status_led_mode == 2 && status_led_on == true)
{
digitalWrite(STATUS_LED, LOW);
status_led_on = false;
}
else if (status_led_mode == 1)
{
digitalWrite(STATUS_LED, HIGH);
status_led_on = true;
}
else
{
digitalWrite(STATUS_LED, LOW);
status_led_on = false;
}
indicator_timestamp = current_timestamp + 200;
}
}