├── .gitattributes ├── .github ├── FUNDING.yml ├── issue_template.md ├── no-response.yml ├── pull_request_template.md └── stale.yml ├── .gitignore ├── BLHeli_S manual SiLabs Rev16.x.pdf ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── Dshotprog spec BLHeli_S.txt ├── LICENSE ├── MakeHexfiles.bat ├── Makefile ├── README.md └── src ├── A.inc ├── B.inc ├── BLHeliBootLoad.inc ├── BLHeliPgm.inc ├── BLHeli_S.asm ├── C.inc ├── D.inc ├── E.inc ├── F.inc ├── G.inc ├── H.inc ├── I.inc ├── J.inc ├── K.inc ├── L.inc ├── M.inc ├── N.inc ├── O.inc ├── P.inc ├── Q.inc ├── R.inc ├── S.inc ├── SI_EFM8BB1_Defs.inc ├── SI_EFM8BB2_Defs.inc ├── SI_EFM8LB1_Defs.inc ├── T.inc ├── U.inc ├── V.inc └── W.inc /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | 24 | *.asm text eol=crlf 25 | *.inc text eol=crlf 26 | *.bat text eol=crlf 27 | 28 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | custom: https://paypal.me/betaflight 2 | patreon: betaflight 3 | -------------------------------------------------------------------------------- /.github/issue_template.md: -------------------------------------------------------------------------------- 1 | # If your issue looks like a hardware fault or a configuration problem please don't raise an issue here. 2 | 3 | ## Please consider using other user support options such as asking the manufacturer of the hardware you are using, RCGroups: https://rcgroups.com/forums/showthread.php?t=2464844, or Slack (registration at https://slack.betaflight.com/) or other user support forums & groups (e.g. Facebook). 4 | 5 | ## If you believe there is an issue with the firmware itself please follow these steps: 6 | 1. Describe your problem; 7 | 2. Include ways to reproduce the problem; 8 | 3. Provide as much information as possible about your hardware and software, including: 9 | - what hardware / software you are using; 10 | - the version of all the software used; 11 | - how things are connected / wired up. 12 | 4. Remove this Text :). 13 | -------------------------------------------------------------------------------- /.github/no-response.yml: -------------------------------------------------------------------------------- 1 | # Configuration for probot-no-response - https://github.com/probot/no-response 2 | 3 | # Number of days of inactivity before an Issue is closed for lack of response 4 | daysUntilClose: 1 5 | # Label requiring a response 6 | responseRequiredLabel: Missing Information 7 | # Comment to post when closing an Issue for lack of response. Set to `false` to disable 8 | closeComment: > 9 | This issue has been automatically closed because the information we asked 10 | to be provided when opening it was not supplied by the original author. 11 | With only the information that is currently in the issue, we don't have 12 | enough information to take action. 13 | -------------------------------------------------------------------------------- /.github/pull_request_template.md: -------------------------------------------------------------------------------- 1 | ## Important considerations when opening a pull request: 2 | 3 | 1. Make sure you do not make the changes you want to open a pull request for on the `master` branch of your fork, or open the pull request from the `master` branch of your fork. Some of our integrations will fail if you do this, resulting in your pull request not being accepted. If this is your first pull request, it is probably a good idea to first read up on how opening pull requests work (https://opensource.com/article/19/7/create-pull-request-github is a good introduction); 4 | 5 | 2. Pull requests will only be accepted if they are opened against the `master` branch of our repository. Pull requests opened against other branches without prior consent from the maintainers will be closed; 6 | 7 | 3. Please follow the coding style guidelines: https://github.com/cleanflight/cleanflight/blob/master/docs/development/CodingStyle.md 8 | 9 | 4. Keep your pull requests as small and concise as possible. One pull request should only ever add / update one feature. If the change that you are proposing has a wider scope, consider splitting it over multiple pull requests. In particular, pull requests that combine changes to features and one or more new targets are not acceptable. 10 | 11 | 5. Ideally, a pull request should contain only one commit, with a descriptive message. If your changes use more than one commit, rebase / squash them into one commit before submitting a pull request. If you need to amend your pull request, make sure that the additional commit has a descriptive message, or - even better - use `git commit --amend` to amend your original commit. 12 | 13 | 6. All pull requests are reviewed. Be ready to receive constructive criticism, and to learn and improve your coding style. Also, be ready to clarify anything that isn't already sufficiently explained in the code and text of the pull request, and to defend your ideas. 14 | 15 | 7. We use continuous integration (CI) with [Travis](https://travis-ci.com/betaflight) to build all targets and run the test suite for every pull request. Pull requests that fail any of the builds or fail tests will most likely not be reviewed before they are fixed to build successfully and pass the tests. In order to get a quick idea if there are things that need fixing **before** opening a pull request or pushing an update into an existing pull request, run `make pre-push` to run a representative subset of the CI build. _Note: This is not an exhaustive test(which will take hours to run on any desktop type system), so even if this passes the CI build might still fail._ 16 | 17 | 8. If your pull request is a fix for one or more issues that are open in GitHub, add a comment to your pull request, and add the issue numbers of the issues that are fixed in the form `Fixes #`. This will cause the issues to be closed when the pull request is merged; 18 | 19 | 9. Remove this Text :). 20 | -------------------------------------------------------------------------------- /.github/stale.yml: -------------------------------------------------------------------------------- 1 | # Configuration for probot-stale - https://github.com/probot/stale 2 | 3 | # Number of days of inactivity before an Issue or Pull Request becomes stale 4 | daysUntilStale: 30 5 | 6 | # Number of days of inactivity before a stale Issue or Pull Request is closed. 7 | # Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale. 8 | daysUntilClose: 7 9 | 10 | # Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable 11 | exemptLabels: 12 | - BUG 13 | - Feature Request 14 | - Pinned 15 | 16 | # Set to true to ignore issues in a project (defaults to false) 17 | exemptProjects: false 18 | 19 | # Set to true to ignore issues in a milestone (defaults to false) 20 | exemptMilestones: true 21 | 22 | # Label to use when marking as stale 23 | staleLabel: Inactive 24 | 25 | # Comment to post when marking as stale. Set to `false` to disable 26 | markComment: > 27 | This issue / pull request has been automatically marked as stale because it 28 | has not had recent activity. It will be closed if no further activity occurs 29 | within a week. 30 | 31 | # Comment to post when removing the stale label. 32 | # unmarkComment: > 33 | # Your comment here. 34 | 35 | # Comment to post when closing a stale Issue or Pull Request. 36 | closeComment: > 37 | Automatically closing as inactive. 38 | 39 | # Limit the number of actions per hour, from 1-30. Default is 30 40 | limitPerRun: 30 41 | 42 | # Limit to only `issues` or `pulls` 43 | # only: issues 44 | 45 | # Optionally, specify configuration settings that are specific to just 'issues' or 'pulls': 46 | # pulls: 47 | # daysUntilStale: 30 48 | # markComment: > 49 | # This pull request has been automatically marked as stale because it has not had 50 | # recent activity. It will be closed if no further activity occurs. Thank you 51 | # for your contributions. 52 | 53 | # issues: 54 | # exemptLabels: 55 | # - confirmed 56 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *~ 3 | *.bak 4 | *.swp 5 | *.save 6 | build/ 7 | src/BLHeli_S.LST 8 | local.mk 9 | -------------------------------------------------------------------------------- /BLHeli_S manual SiLabs Rev16.x.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/betaflight/BLHeli_S/5df60609e2808709dd868781c809f0b3b9e66f61/BLHeli_S manual SiLabs Rev16.x.pdf -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | In the interest of fostering an open and welcoming environment, we as contributors and maintainers pledge to making participation in our project and our community a harassment-free experience for everyone, regardless of age, body size, disability, ethnicity, gender identity and expression, level of experience, nationality, personal appearance, race, religion, or sexual identity and orientation. 6 | 7 | ## Our Standards 8 | 9 | Examples of behavior that contributes to creating a positive environment include: 10 | 11 | * Using welcoming and inclusive language 12 | * Being respectful of differing viewpoints and experiences 13 | * Gracefully accepting constructive criticism 14 | * Focusing on what is best for the community 15 | * Showing empathy towards other community members 16 | 17 | Examples of unacceptable behavior by participants include: 18 | 19 | * The use of sexualized language or imagery and unwelcome sexual attention or advances 20 | * Trolling, insulting/derogatory comments, and personal or political attacks 21 | * Public or private harassment 22 | * Publishing others' private information, such as a physical or electronic address, without explicit permission 23 | * Other conduct which could reasonably be considered inappropriate in a professional setting 24 | 25 | ## Our Responsibilities 26 | 27 | Project maintainers are responsible for clarifying the standards of acceptable behavior and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behavior. 28 | 29 | Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, or to ban temporarily or permanently any contributor for other behaviors that they deem inappropriate, threatening, offensive, or harmful. 30 | 31 | ## Scope 32 | 33 | This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. Examples of representing a project or community include using an official project e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. Representation of a project may be further defined and clarified by project maintainers. 34 | 35 | ## Enforcement 36 | 37 | Instances of abusive, harassing, or otherwise unacceptable behavior may be reported by contacting the project team at conduct-violations@betaflight.com. The project team will review and investigate all complaints, and will respond in a way that it deems appropriate to the circumstances. The project team is obligated to maintain confidentiality with regard to the reporter of an incident. Further details of specific enforcement policies may be posted separately. 38 | 39 | Project maintainers who do not follow or enforce the Code of Conduct in good faith may face temporary or permanent repercussions as determined by other members of the project's leadership. 40 | 41 | ## Attribution 42 | 43 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, available at [http://contributor-covenant.org/version/1/4][version] 44 | 45 | [homepage]: http://contributor-covenant.org 46 | [version]: http://contributor-covenant.org/version/1/4/ 47 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Issues and Support. 2 | 3 | Please remember the issue tracker on github is _not_ for user support. Please also do not email developers directly for support. Instead please use IRC or the forums first, then if the problem is confirmed create an issue that details how to repeat the problem so it can be investigated. 4 | 5 | Issues created without steps to repeat are likely to be closed. E-mail requests for support will go un-answered; All support needs to be public so that other people can read the problems and solutions. 6 | 7 | Remember that issues that are due to mis-configuration, wiring or failure to read documentation just takes time away from the developers and can often be solved without developer interaction by other users. 8 | 9 | Please search for existing issues *before* creating new ones. 10 | 11 | # Developers 12 | 13 | Please refer to the development section in the [this folder](https://github.com/betaflight/betaflight/tree/master/docs/development). 14 | -------------------------------------------------------------------------------- /Dshotprog spec BLHeli_S.txt: -------------------------------------------------------------------------------- 1 | 0 DSHOT_CMD_MOTOR_STOP, // Currently not implemented 2 | 1 DSHOT_CMD_BEEP1, // Wait at least length of beep (380ms) before next command 3 | 2 DSHOT_CMD_BEEP2, // Wait at least length of beep (380ms) before next command 4 | 3 DSHOT_CMD_BEEP3, // Wait at least length of beep (400ms) before next command 5 | 4 DSHOT_CMD_BEEP4, // Wait at least length of beep (400ms) before next command 6 | 5 DSHOT_CMD_BEEP5, // Wait at least length of beep (400ms) before next command 7 | 6 DSHOT_CMD_ESC_INFO, // Currently not implemented 8 | 7 DSHOT_CMD_SPIN_DIRECTION_1, // Need 6x, no wait required 9 | 8 DSHOT_CMD_SPIN_DIRECTION_2, // Need 6x, no wait required 10 | 9 DSHOT_CMD_3D_MODE_OFF, // Need 6x, no wait required 11 | 10 DSHOT_CMD_3D_MODE_ON, // Need 6x, no wait required 12 | 11 DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented 13 | 12 DSHOT_CMD_SAVE_SETTINGS, // Need 6x, wait at least 12ms before next command 14 | 20 DSHOT_CMD_SPIN_DIRECTION_NORMAL, // Need 6x, no wait required 15 | 21 DSHOT_CMD_SPIN_DIRECTION_REVERSED, // Need 6x, no wait required 16 | 22 DSHOT_CMD_LED0_ON, // Currently not implemented 17 | 23 DSHOT_CMD_LED1_ON, // Currently not implemented 18 | 24 DSHOT_CMD_LED2_ON, // Currently not implemented 19 | 25 DSHOT_CMD_LED3_ON, // Currently not implemented 20 | 26 DSHOT_CMD_LED0_OFF, // Currently not implemented 21 | 27 DSHOT_CMD_LED1_OFF, // Currently not implemented 22 | 28 DSHOT_CMD_LED2_OFF, // Currently not implemented 23 | 29 DSHOT_CMD_LED3_OFF, // Currently not implemented 24 | DSHOT_CMD_MAX = 47 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /MakeHexfiles.bat: -------------------------------------------------------------------------------- 1 | @ECHO off 2 | @ECHO ***** Batch file for BlHeli_S (from 4712) v.2 ***** 3 | @ECHO ***** All Messages will be saved to MakeHex_Result.txt ***** 4 | @ECHO ***** Start compile with any key - CTRL-C to abort ***** 5 | Break ON 6 | @pause 7 | DEL MakeHex_Result.txt /Q 8 | 9 | rem ***** Adapt settings to your enviroment **** 10 | DEL Output\Hex\*.* /Q 11 | RMDIR Output\Hex 12 | DEL Output\*.* /Q 13 | RMDIR Output 14 | MKDIR Output 15 | MKDIR Output\Hex 16 | SET Revision=REV16_7 17 | SET KeilPath=C:\SiliconLabs\SimplicityStudio\v4\developer\toolchains\keil_8051\9.53\BIN 18 | 19 | @ECHO Revision: %Revision% >> MakeHex_Result.txt 20 | @ECHO Path for Keil toolchain: %KeilPath% >> MakeHex_Result.txt 21 | @ECHO Start compile ..... >> MakeHex_Result.txt 22 | 23 | 24 | SET ESCNO=1 25 | SET ESC=A_L_ 26 | SET MCU_48MHZ=0 27 | call:compile 28 | SET ESC=A_H_ 29 | SET MCU_48MHZ=1 30 | call:compile 31 | SET /A ESCNO+=1 32 | 33 | SET ESC=B_L_ 34 | SET MCU_48MHZ=0 35 | call:compile 36 | SET ESC=B_H_ 37 | SET MCU_48MHZ=1 38 | call:compile 39 | SET /A ESCNO+=1 40 | 41 | SET ESC=C_L_ 42 | SET MCU_48MHZ=0 43 | call:compile 44 | SET ESC=C_H_ 45 | SET MCU_48MHZ=1 46 | call:compile 47 | SET /A ESCNO+=1 48 | 49 | SET ESC=D_L_ 50 | SET MCU_48MHZ=0 51 | call:compile 52 | SET ESC=D_H_ 53 | SET MCU_48MHZ=1 54 | call:compile 55 | SET /A ESCNO+=1 56 | 57 | SET ESC=E_L_ 58 | SET MCU_48MHZ=0 59 | call:compile 60 | SET ESC=E_H_ 61 | SET MCU_48MHZ=1 62 | call:compile 63 | SET /A ESCNO+=1 64 | 65 | SET ESC=F_L_ 66 | SET MCU_48MHZ=0 67 | call:compile 68 | SET ESC=F_H_ 69 | SET MCU_48MHZ=1 70 | call:compile 71 | SET /A ESCNO+=1 72 | 73 | SET ESC=G_L_ 74 | SET MCU_48MHZ=0 75 | call:compile 76 | SET ESC=G_H_ 77 | SET MCU_48MHZ=1 78 | call:compile 79 | SET /A ESCNO+=1 80 | 81 | SET ESC=H_L_ 82 | SET MCU_48MHZ=0 83 | call:compile 84 | SET ESC=H_H_ 85 | SET MCU_48MHZ=1 86 | call:compile 87 | SET /A ESCNO+=1 88 | 89 | SET ESC=I_L_ 90 | SET MCU_48MHZ=0 91 | call:compile 92 | SET ESC=I_H_ 93 | SET MCU_48MHZ=1 94 | call:compile 95 | SET /A ESCNO+=1 96 | 97 | SET ESC=J_L_ 98 | SET MCU_48MHZ=0 99 | call:compile 100 | SET ESC=J_H_ 101 | SET MCU_48MHZ=1 102 | call:compile 103 | SET /A ESCNO+=1 104 | 105 | SET ESC=K_L_ 106 | SET MCU_48MHZ=0 107 | call:compile 108 | SET ESC=K_H_ 109 | SET MCU_48MHZ=1 110 | call:compile 111 | SET /A ESCNO+=1 112 | 113 | SET ESC=L_L_ 114 | SET MCU_48MHZ=0 115 | call:compile 116 | SET ESC=L_H_ 117 | SET MCU_48MHZ=1 118 | call:compile 119 | SET /A ESCNO+=1 120 | 121 | SET ESC=M_L_ 122 | SET MCU_48MHZ=0 123 | call:compile 124 | SET ESC=M_H_ 125 | SET MCU_48MHZ=1 126 | call:compile 127 | SET /A ESCNO+=1 128 | 129 | SET ESC=N_L_ 130 | SET MCU_48MHZ=0 131 | call:compile 132 | SET ESC=N_H_ 133 | SET MCU_48MHZ=1 134 | call:compile 135 | SET /A ESCNO+=1 136 | 137 | SET ESC=O_L_ 138 | SET MCU_48MHZ=0 139 | call:compile 140 | SET ESC=O_H_ 141 | SET MCU_48MHZ=1 142 | call:compile 143 | SET /A ESCNO+=1 144 | 145 | SET ESC=P_L_ 146 | SET MCU_48MHZ=0 147 | call:compile 148 | SET ESC=P_H_ 149 | SET MCU_48MHZ=1 150 | call:compile 151 | SET /A ESCNO+=1 152 | 153 | SET ESC=Q_L_ 154 | SET MCU_48MHZ=0 155 | call:compile 156 | SET ESC=Q_H_ 157 | SET MCU_48MHZ=1 158 | call:compile 159 | SET /A ESCNO+=1 160 | 161 | SET ESC=R_L_ 162 | SET MCU_48MHZ=0 163 | call:compile 164 | SET ESC=R_H_ 165 | SET MCU_48MHZ=1 166 | call:compile 167 | SET /A ESCNO+=1 168 | 169 | SET ESC=S_L_ 170 | SET MCU_48MHZ=0 171 | call:compile 172 | SET ESC=S_H_ 173 | SET MCU_48MHZ=1 174 | call:compile 175 | SET /A ESCNO+=1 176 | 177 | SET ESC=T_L_ 178 | SET MCU_48MHZ=0 179 | call:compile 180 | SET ESC=T_H_ 181 | SET MCU_48MHZ=1 182 | call:compile 183 | SET /A ESCNO+=1 184 | 185 | SET ESC=U_L_ 186 | SET MCU_48MHZ=0 187 | call:compile 188 | SET ESC=U_H_ 189 | SET MCU_48MHZ=1 190 | call:compile 191 | SET /A ESCNO+=1 192 | 193 | SET ESC=V_L_ 194 | SET MCU_48MHZ=0 195 | call:compile 196 | SET ESC=V_H_ 197 | SET MCU_48MHZ=1 198 | call:compile 199 | SET /A ESCNO+=1 200 | 201 | SET ESC=W_L_ 202 | SET MCU_48MHZ=0 203 | call:compile 204 | SET ESC=W_H_ 205 | SET MCU_48MHZ=1 206 | call:compile 207 | SET /A ESCNO+=1 208 | 209 | 210 | goto :end 211 | 212 | 213 | :compile 214 | SET FETON_DELAY=0 215 | SET ESCNAME=%ESC%%FETON_DELAY% 216 | call :compile_code 217 | SET /A FETON_DELAY=5 218 | SET ESCNAME=%ESC%%FETON_DELAY% 219 | call :compile_code 220 | SET /A FETON_DELAY=10 221 | SET ESCNAME=%ESC%%FETON_DELAY% 222 | call :compile_code 223 | SET /A FETON_DELAY=15 224 | SET ESCNAME=%ESC%%FETON_DELAY% 225 | call :compile_code 226 | SET /A FETON_DELAY=20 227 | SET ESCNAME=%ESC%%FETON_DELAY% 228 | call :compile_code 229 | SET /A FETON_DELAY=25 230 | SET ESCNAME=%ESC%%FETON_DELAY% 231 | call :compile_code 232 | SET /A FETON_DELAY=30 233 | SET ESCNAME=%ESC%%FETON_DELAY% 234 | call :compile_code 235 | SET /A FETON_DELAY=40 236 | SET ESCNAME=%ESC%%FETON_DELAY% 237 | call :compile_code 238 | SET /A FETON_DELAY=50 239 | SET ESCNAME=%ESC%%FETON_DELAY% 240 | call :compile_code 241 | SET /A FETON_DELAY=70 242 | SET ESCNAME=%ESC%%FETON_DELAY% 243 | call :compile_code 244 | SET /A FETON_DELAY=90 245 | SET ESCNAME=%ESC%%FETON_DELAY% 246 | call :compile_code 247 | goto :eof 248 | 249 | 250 | :compile_code 251 | @ECHO compiling %ESCNAME% 252 | @ECHO. >> MakeHex_Result.txt 253 | @ECHO ******************************************************************** >> MakeHex_Result.txt 254 | @ECHO %ESCNAME% >> MakeHex_Result.txt 255 | @ECHO ******************************************************************** >> MakeHex_Result.txt 256 | %KeilPath%\AX51.exe "BLHeli_S.asm" DEFINE(ESCNO=%ESCNO%) DEFINE(MCU_48MHZ=%MCU_48MHZ%) DEFINE(FETON_DELAY=%FETON_DELAY%) OBJECT(Output\%ESCNAME%_%Revision%.OBJ) DEBUG MACRO NOMOD51 COND SYMBOLS PAGEWIDTH(120) PAGELENGTH(65) >> MakeHex_Result.txt 257 | %KeilPath%\LX51.exe "Output\%ESCNAME%_%Revision%.OBJ" TO "Output\%ESCNAME%_%Revision%.OMF" PAGEWIDTH (120) PAGELENGTH (65) >> MakeHex_Result.txt 258 | %KeilPath%\Ohx51 "Output\%ESCNAME%_%Revision%.OMF" "HEXFILE (Output\%ESCNAME%_%Revision%.HEX)" "H386" >> MakeHex_Result.txt 259 | copy "Output\%ESCNAME%_%Revision%.HEX" "Output\Hex\%ESCNAME%_%Revision%.HEX" > nul 260 | del "Output\%ESCNAME%_%Revision%.HEX" > nul 261 | @ECHO. >> MakeHex_Result.txt 262 | goto :eof 263 | 264 | :end 265 | 266 | @pause 267 | exit 268 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # set current revision 2 | REVISION ?= REV16_7 3 | 4 | # targets 5 | TARGETS = A B C D E F G H I J K L M N O P Q R S T U V W 6 | MCUS = H L 7 | FETON_DELAYS = 0 5 10 15 20 25 30 40 50 70 90 8 | 9 | # example single target 10 | VARIANT ?= A 11 | MCU ?= H 12 | FETON_DELAY ?= 5 13 | 14 | # configure the script to use the wine installation delivered with 15 | # SimplicityStudio. these wine settings are quite important. if you get 16 | # ERROR L250: CODE SIZE LIMIT IN RESTRICTED VERSION EXCEEDED 17 | # you messed up your simplicity studio install/path settins below: 18 | SIMPLICITY_PATH ?= ~/local/SimplicityStudio_v4/ 19 | WINE_PREFIX ?= ~/.config/SimplicityStudio/v4/studio-wine 20 | WINE_BIN ?= $(SIMPLICITY_PATH)/support/common/wine/usr/bin/wine 21 | WINE = export WINEPREFIX=`realpath $(WINE_PREFIX)`; $(WINE_BIN) 22 | 23 | # path to the keil binaries 24 | KEIL_PATH = $(SIMPLICITY_PATH)/developer/toolchains/keil_8051/9.53/BIN 25 | 26 | # some directory config 27 | SOURCE_DIR ?= src 28 | OUTPUT_DIR ?= build 29 | OUTPUT_DIR_HEX ?= $(OUTPUT_DIR)/hex 30 | LOG_DIR ?= $(OUTPUT_DIR)/log 31 | 32 | # define the assembler/linker scripts 33 | AX51_BIN = $(KEIL_PATH)/AX51.exe 34 | LX51_BIN = $(KEIL_PATH)/LX51.exe 35 | OX51_BIN = $(KEIL_PATH)/Ohx51.exe 36 | AX51 = $(WINE) $(AX51_BIN) 37 | LX51 = $(WINE) $(LX51_BIN) 38 | OX51 = $(WINE) $(OX51_BIN) 39 | 40 | # set up flags 41 | AX51_FLAGS = DEBUG MACRO NOMOD51 COND SYMBOLS PAGEWIDTH(120) PAGELENGTH(65) 42 | LX51_FLAGS = PAGEWIDTH (120) PAGELENGTH (65) 43 | 44 | # set up sources 45 | ASM_SRC = $(SOURCE_DIR)/BLHeli_S.asm 46 | ASM_INC = $(addsuffix .inc,$(addprefix $(SOURCE_DIR)/,$(TARGETS))) $(SOURCE_DIR)/BLHeliBootLoad.inc $(SOURCE_DIR)/BLHeliPgm.inc $(SOURCE_DIR)/SI_EFM8BB1_Defs.inc $(SOURCE_DIR)/SI_EFM8BB2_Defs.inc 47 | 48 | # check that wine/simplicity studio is available 49 | EXECUTABLES = $(WINE_BIN) $(AX51_BIN) $(LX51_BIN) $(OX51_BIN) 50 | DUMMYVAR := $(foreach exec, $(EXECUTABLES), \ 51 | $(if $(wildcard $(exec)),found, \ 52 | $(error "Could not find $(exec). Make sure to set the correct paths to the simplicity install location"))) 53 | 54 | # make sure the list of obj files is expanded twice 55 | .SECONDEXPANSION: 56 | OBJS = 57 | 58 | define MAKE_OBJ 59 | OBJS += $(OUTPUT_DIR)/$(1)_$(2)_$(3)_$(REVISION).OBJ 60 | $(OUTPUT_DIR)/$(1)_$(2)_$(3)_$(REVISION).OBJ : $(ASM_SRC) $(ASM_INC) 61 | $(eval _ESC := $(1)) 62 | $(eval _ESC_INT := $(shell printf "%d" "'${_ESC}")) 63 | $(eval _ESCNO := $(shell echo $$(( $(_ESC_INT) - 65 + 1)))) 64 | $(eval _MCU_48MHZ := $(subst L,0,$(subst H,1,$(2)))) 65 | $(eval _FETON_DELAY := $(3)) 66 | $(eval _LOG := $(LOG_DIR)/$(1)_$(2)_$(3)_$(REVISION).log) 67 | @mkdir -p $(OUTPUT_DIR) 68 | @mkdir -p $(LOG_DIR) 69 | @echo "AX51 : $$@" 70 | @$(AX51) $(ASM_SRC) \ 71 | "DEFINE(ESCNO=$(_ESCNO)) " \ 72 | "DEFINE(MCU_48MHZ=$(_MCU_48MHZ)) "\ 73 | "DEFINE(FETON_DELAY=$(_FETON_DELAY)) "\ 74 | "OBJECT($$@) "\ 75 | "$(AX51_FLAGS)" >> $(_LOG) 2>&1; test $$$$? -lt 2 || tail $(_LOG) 76 | 77 | endef 78 | 79 | HEX_TARGETS = $(OBJS:.OBJ=.HEX) 80 | 81 | EFM8_LOAD_BIN ?= efm8load.py 82 | EFM8_LOAD_PORT ?= /dev/ttyUSB0 83 | EFM8_LOAD_BAUD ?= 57600 84 | 85 | SINGLE_TARGET_HEX = $(OUTPUT_DIR)/$(VARIANT)_$(MCU)_$(FETON_DELAY)_$(REVISION).HEX 86 | 87 | single_target : $(SINGLE_TARGET_HEX) 88 | 89 | all : $$(HEX_TARGETS) 90 | @echo "\nbuild finished. built $(shell ls -l $(OUTPUT_DIR_HEX) | wc -l) hex targets\n" 91 | 92 | # create all obj targets using macro expansion 93 | $(foreach _e,$(TARGETS), \ 94 | $(foreach _m, $(MCUS), \ 95 | $(foreach _f, $(FETON_DELAYS), \ 96 | $(eval $(call MAKE_OBJ,$(_e),$(_m),$(_f)))))) 97 | 98 | 99 | $(OUTPUT_DIR)/%.OMF : $(OUTPUT_DIR)/%.OBJ 100 | $(eval LOG := $(LOG_DIR)/$(basename $(notdir $@)).log) 101 | @echo "LX51 : linking $< to $@" 102 | @$(LX51) "$<" TO "$@" "$(LX51_FLAGS)" >> $(LOG) 2>&1; test $$? -lt 2 || tail $(LOG) 103 | 104 | $(OUTPUT_DIR)/%.HEX : $(OUTPUT_DIR)/%.OMF 105 | $(eval LOG := $(LOG_DIR)/$(basename $(notdir $@)).log) 106 | @mkdir -p $(OUTPUT_DIR_HEX) 107 | @echo "OHX : generating hex file $@" 108 | @$(OX51) "$<" "HEXFILE ($@)" "H386" >> $(LOG) 2>&1; test $$? -lt 2 || tail $(LOG) 109 | @cp $@ $(OUTPUT_DIR_HEX)/$(notdir $@) 110 | 111 | help: 112 | @echo "" 113 | @echo "usage examples:" 114 | @echo "=================================================================" 115 | @echo "make all # build all targets" 116 | @echo "make VARIANT=A MCU=H FETON_DELAY=5 # to build a single target" 117 | @echo 118 | 119 | clean: 120 | @rm -rf $(LOG_DIR)/* 121 | @rm -rf $(OUTPUT_DIR)/* 122 | 123 | efm8load: single_target 124 | $(EFM8_LOAD_BIN) -p $(EFM8_LOAD_PORT) -b $(EFM8_LOAD_BAUD) -w $(SINGLE_TARGET_HEX) 125 | 126 | 127 | .PHONY: all clean help efm8load 128 | 129 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This is a fork of the BLHeli\_S open source sensorless brushless motor electronic speed control (ESC) firmware. 2 | It was created in an attempt to revive open source development of this type of firmware, and make modern features available to users of open source software. 3 | 4 | For flashing and configuration, get the latest version of BLHeli Configurator from [here](https://github.com/blheli-configurator/blheli-configurator/releases). 5 | The latest releases of BLHeli\_S are downloadable from within the Configurator, or the hex files can be downloaded from [here](https://github.com/betaflight/BLHeli_S/releases). 6 | 7 | For more information, check out these threads: 8 | https://www.rcgroups.com/forums/showthread.php?2640796 (for BLHeli\_S) 9 | http://www.rcgroups.com/forums/showthread.php?t=2136895 (for BLHeli) 10 | -------------------------------------------------------------------------------- /src/A.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "A" 26 | ; X X RC X MC MB MA CC X X Cc Cp Bc Bp Ac Ap 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#A_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#A_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#A_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#A_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#A_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#A_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#A_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#A_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#A_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#A_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#A_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#A_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#A_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#A_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#A_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#A_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#A_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#A_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#A_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#A_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#A_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#A_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Mux_C EQU 3 ;i 134 | Mux_B EQU 2 ;i 135 | Mux_A EQU 1 ;i 136 | Comp_Com EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | 316 | ;********************* 317 | ; PORT 1 definitions * 318 | ;********************* 319 | ; EQU 7 ;i 320 | ; EQU 6 ;i 321 | CcomFET EQU 5 ;o 322 | CpwmFET EQU 4 ;o 323 | BcomFET EQU 3 ;o 324 | BpwmFET EQU 2 ;o 325 | AcomFET EQU 1 ;o 326 | ApwmFET EQU 0 ;o 327 | 328 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 329 | P1_INIT EQU 00h 330 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_SKIP EQU 3Fh 332 | 333 | ApwmFET_on MACRO 334 | setb P1.ApwmFET 335 | IF FETON_DELAY == 0 336 | setb P1.AcomFET 337 | ENDIF 338 | ENDM 339 | ApwmFET_off MACRO 340 | IF FETON_DELAY != 0 341 | clr P1.ApwmFET 342 | ELSE 343 | clr P1.AcomFET 344 | ENDIF 345 | ENDM 346 | BpwmFET_on MACRO 347 | setb P1.BpwmFET 348 | IF FETON_DELAY == 0 349 | setb P1.BcomFET 350 | ENDIF 351 | ENDM 352 | BpwmFET_off MACRO 353 | IF FETON_DELAY != 0 354 | clr P1.BpwmFET 355 | ELSE 356 | clr P1.BcomFET 357 | ENDIF 358 | ENDM 359 | CpwmFET_on MACRO 360 | setb P1.CpwmFET 361 | IF FETON_DELAY == 0 362 | setb P1.CcomFET 363 | ENDIF 364 | ENDM 365 | CpwmFET_off MACRO 366 | IF FETON_DELAY != 0 367 | clr P1.CpwmFET 368 | ELSE 369 | clr P1.CcomFET 370 | ENDIF 371 | ENDM 372 | All_pwmFETs_Off MACRO 373 | IF FETON_DELAY != 0 374 | clr P1.ApwmFET 375 | clr P1.BpwmFET 376 | clr P1.CpwmFET 377 | ELSE 378 | clr P1.AcomFET 379 | clr P1.BcomFET 380 | clr P1.CcomFET 381 | ENDIF 382 | ENDM 383 | 384 | AcomFET_on MACRO 385 | IF FETON_DELAY == 0 386 | clr P1.ApwmFET 387 | ENDIF 388 | setb P1.AcomFET 389 | ENDM 390 | AcomFET_off MACRO 391 | clr P1.AcomFET 392 | ENDM 393 | BcomFET_on MACRO 394 | IF FETON_DELAY == 0 395 | clr P1.BpwmFET 396 | ENDIF 397 | setb P1.BcomFET 398 | ENDM 399 | BcomFET_off MACRO 400 | clr P1.BcomFET 401 | ENDM 402 | CcomFET_on MACRO 403 | IF FETON_DELAY == 0 404 | clr P1.CpwmFET 405 | ENDIF 406 | setb P1.CcomFET 407 | ENDM 408 | CcomFET_off MACRO 409 | clr P1.CcomFET 410 | ENDM 411 | All_comFETs_Off MACRO 412 | clr P1.AcomFET 413 | clr P1.BcomFET 414 | clr P1.CcomFET 415 | ENDM 416 | 417 | Set_Pwm_A MACRO 418 | IF FETON_DELAY == 0 419 | setb P1.AcomFET 420 | mov P1SKIP, #3Eh 421 | ELSE 422 | mov P1SKIP, #3Ch 423 | ENDIF 424 | ENDM 425 | Set_Pwm_B MACRO 426 | IF FETON_DELAY == 0 427 | setb P1.BcomFET 428 | mov P1SKIP, #3Bh 429 | ELSE 430 | mov P1SKIP, #33h 431 | ENDIF 432 | ENDM 433 | Set_Pwm_C MACRO 434 | IF FETON_DELAY == 0 435 | setb P1.CcomFET 436 | mov P1SKIP, #2Fh 437 | ELSE 438 | mov P1SKIP, #0Fh 439 | ENDIF 440 | ENDM 441 | Set_Pwms_Off MACRO 442 | mov P1SKIP, #3Fh 443 | ENDM 444 | 445 | Set_Comp_Phase_A MACRO 446 | mov CMP0MX, #10h ; Set comparator multiplexer to phase A 447 | ENDM 448 | Set_Comp_Phase_B MACRO 449 | mov CMP0MX, #20h ; Set comparator multiplexer to phase B 450 | ENDM 451 | Set_Comp_Phase_C MACRO 452 | mov CMP0MX, #30h ; Set comparator multiplexer to phase C 453 | ENDM 454 | Read_Comp_Out MACRO 455 | mov A, CMP0CN0 ; Read comparator output 456 | ENDM 457 | 458 | 459 | ;********************* 460 | ; PORT 2 definitions * 461 | ;********************* 462 | DebugPin EQU 0 ;o 463 | 464 | P2_PUSHPULL EQU (1 SHL DebugPin) 465 | 466 | 467 | ;********************** 468 | ; MCU specific macros * 469 | ;********************** 470 | Interrupt_Table_Definition MACRO 471 | CSEG AT 0 ; Code segment start 472 | jmp reset 473 | CSEG AT 03h ; Int0 interrupt 474 | jmp int0_int 475 | IF MCU_48MHZ == 1 476 | CSEG AT 0Bh ; Timer0 overflow interrupt 477 | jmp t0_int 478 | ENDIF 479 | CSEG AT 13h ; Int1 interrupt 480 | jmp int1_int 481 | CSEG AT 1Bh ; Timer1 overflow interrupt 482 | jmp t1_int 483 | CSEG AT 2Bh ; Timer2 overflow interrupt 484 | jmp t2_int 485 | CSEG AT 5Bh ; Pca interrupt 486 | jmp pca_int 487 | CSEG AT 73h ; Timer3 overflow/compare interrupt 488 | jmp t3_int 489 | ENDM 490 | 491 | Initialize_Xbar MACRO 492 | mov XBR2, #40h ; Xbar enabled 493 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 494 | ENDM 495 | 496 | Initialize_Comparator MACRO 497 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 498 | mov CMP0MD, #00h ; Comparator response time 100ns 499 | ENDM 500 | Initialize_Adc MACRO 501 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 502 | IF MCU_48MHZ == 0 503 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 504 | ELSE 505 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 506 | ENDIF 507 | mov ADC0MX, #10h ; Select temp sensor input 508 | mov ADC0CN0, #80h ; ADC enabled 509 | mov ADC0CN1, #01h ; Common mode buffer enabled 510 | ENDM 511 | Start_Adc MACRO 512 | mov ADC0CN0, #90h ; ADC start 513 | ENDM 514 | Read_Adc_Result MACRO 515 | mov Temp1, ADC0L 516 | mov Temp2, ADC0H 517 | ENDM 518 | Stop_Adc MACRO 519 | ENDM 520 | Set_RPM_Out MACRO 521 | ENDM 522 | Clear_RPM_Out MACRO 523 | ENDM 524 | Set_MCU_Clk_24MHz MACRO 525 | mov CLKSEL, #13h ; Set clock to 24MHz 526 | mov SFRPAGE, #10h 527 | mov PFE0CN, #00h ; Set flash timing for 24MHz 528 | mov SFRPAGE, #00h 529 | mov Clock_Set_At_48MHz, #0 530 | ENDM 531 | Set_MCU_Clk_48MHz MACRO 532 | mov SFRPAGE, #10h 533 | mov PFE0CN, #30h ; Set flash timing for 48MHz 534 | mov SFRPAGE, #00h 535 | mov CLKSEL, #03h ; Set clock to 48MHz 536 | mov Clock_Set_At_48MHz, #1 537 | ENDM 538 | Set_LED_0 MACRO 539 | ENDM 540 | Clear_LED_0 MACRO 541 | ENDM 542 | Set_LED_1 MACRO 543 | ENDM 544 | Clear_LED_1 MACRO 545 | ENDM 546 | Set_LED_2 MACRO 547 | ENDM 548 | Clear_LED_2 MACRO 549 | ENDM 550 | Set_LED_3 MACRO 551 | ENDM 552 | Clear_LED_3 MACRO 553 | ENDM 554 | -------------------------------------------------------------------------------- /src/BLHeliBootLoad.inc: -------------------------------------------------------------------------------- 1 | ; BLHeli bootloader for SiLabs MCUs. Based upon AVRootloader (copyright HR) 2 | 3 | XTAL EQU 25000000 4 | 5 | BOOT_START EQU 1C00h ; Bootloader segment address 6 | BOOT_DELAY EQU XTAL/4 ; About 250ms (don't set to fast to avoid connection problems) 7 | BOOT_BAUDRATE EQU 19200 ; Only used if no baudrate detection activated, XTAL is than important 8 | BOOT_VERSION EQU 6 ; Version 6 (must be not changed) 9 | BOOT_PAGES EQU 1 ; Number of flash segments for bootloader 10 | 11 | UART_LOOP EQU 26 ; Depends upon timing of putc, getc 12 | BAUDTIME EQU ((XTAL/BOOT_BAUDRATE)/3)-UART_LOOP 13 | 14 | SUCCESS EQU 030h 15 | ERRORVERIFY EQU 0C0h 16 | ERRORCOMMAND EQU 0C1h 17 | ERRORCRC EQU 0C2h 18 | ERRORPROG EQU 0C5h 19 | 20 | POLYNOM EQU 0A001h ; CRC Polynom 21 | 22 | Xl EQU R0 ; Temporary X 23 | Xh EQU R1 24 | Paral EQU R2 ; Params for UART 25 | Parah EQU R3 26 | Cmdl EQU R4 ; Commands 27 | Cmdh EQU R5 28 | Cntl EQU R6 ; Baudtime 29 | Cnth EQU R7 30 | 31 | DSEG AT 20h 32 | Bit_Reg: DS 1 ; Bit storage register 33 | Byte_Reg: DS 1 ; Byte storage register 34 | Crcl: DS 1 ; CRC 16Bit 35 | Crch: DS 1 36 | Baudl: DS 1 ; Baudtime 37 | Baudh: DS 1 38 | Bit_Cnt: DS 1 ; Counter in UART loops 39 | Byte_Cntl: DS 1 ; Generic counter 40 | Byte_Cnth: DS 1 41 | BL_Flash_Key_1: DS 1 ; Flash keys 42 | BL_Flash_Key_2: DS 1 43 | 44 | CSEG AT BOOT_START ; Bootloader start 45 | init:clr IE_EA 46 | ; Select register bank 0 for main program routines 47 | clr PSW.3 ; Select register bank 0 for main program routines 48 | ; Disable the WDT. 49 | mov WDTCN, #0DEh ; Disable watchdog 50 | mov WDTCN, #0ADh 51 | ; Initialize stack 52 | mov SP, #0c0h ; Stack = 64 upper bytes of RAM 53 | ; Initialize clock 54 | mov CLKSEL, #00h ; Set clock divider to 1 55 | ; Initialize VDD monitor 56 | orl VDM0CN, #080h ; Enable the VDD monitor 57 | mov Baudl, #38h ; Wait 100us 58 | mov Baudh, #03h 59 | acall waitf 60 | ; Initialize flash keys 61 | mov BL_Flash_Key_1, #0A5h ; First key code 62 | mov BL_Flash_Key_2, #0F1h ; Second key code 63 | ; Initialize ports 64 | orl RTX_MDIN, #(1 SHL RTX_PIN) ; Set digital 65 | anl RTX_MDOUT, #NOT(1 SHL RTX_PIN) ; Disable pushpull 66 | setb RTX_PORT.RTX_PIN ; Set data high 67 | mov RTX_SKIP, #0FFh 68 | mov XBR2, #40h; ; Enable crossbar 69 | ; Set number of connect attempts before exiting bootloader 70 | mov Cmdh, #250 71 | 72 | ; Identifier scanning 73 | abd: mov Xl, #(low(BOOT_DELAY / 6)+1) 74 | mov Xh, #(high(BOOT_DELAY / 6)+1) 75 | mov Cmdl, #(high((BOOT_DELAY / 6) SHR 8)+1) 76 | mov Crcl, #0 77 | mov Crch, #0 78 | mov DPTR, #BOOT_SIGN 79 | mov Parah, #(BOOT_MSG - BOOT_SIGN) 80 | mov Baudl, #low(BAUDTIME) 81 | mov Baudh, #high(BAUDTIME) 82 | 83 | wait_for_low: 84 | jnb RTX_PORT.RTX_PIN, ($+5) 85 | ajmp wait_for_low 86 | 87 | ; Identifier (BOOT_SIGN) scanning with timeout and checksum 88 | id1: jb RTX_PORT.RTX_PIN, id3 ; Look for high 89 | djnz Xl, id1 ; Subtract 1 from X (BOOT_DELAY) 90 | djnz Xh, id1 91 | djnz Cmdl, id1 92 | 93 | ajmp exit 94 | 95 | id3: jnb RTX_PORT.RTX_PIN, id4 ; Look for low 96 | djnz Xl, id3 ; Subtract 1 from X (BOOT_DELAY) 97 | djnz Xh, id3 98 | djnz Cmdl, id3 99 | 100 | ajmp exit 101 | 102 | id4: acall getx ; Read character 103 | clr A 104 | movc A, @A+DPTR ; Load BOOT_SIGN character 105 | inc DPTR 106 | clr C 107 | subb A, Paral ; Compare with read character 108 | jz id5 109 | djnz Cmdh, abd ; Retry if not last connect attempt 110 | ajmp exit 111 | 112 | id5: 113 | djnz Parah, id1 114 | 115 | acall getw ; Read CRC 116 | jz ($+4) ; Check CRC 117 | ajmp abd 118 | 119 | ; Send info about chip/bootloader (BOOT_MSG + BOOT_INFO) 120 | mov Parah, #((BOOT_INFO - BOOT_MSG) + 4) 121 | in1: clr A 122 | movc A, @A+DPTR ; Load character 123 | mov Paral, A 124 | inc DPTR 125 | acall putc 126 | djnz Parah, in1 127 | 128 | 129 | ; Main commandloop 130 | ; 0=Run/restart 131 | ; 1=Program flash, 2=Erase flash, 3=Read flash 132 | ; 0xFF=Set address, 0xFE=Set buffer, 0xFD=Keep alive 133 | main:mov Paral, #SUCCESS 134 | mai1:acall putc 135 | mov Crcl, #0 ; Reset CRC 136 | mov Crch, #0 137 | acall getw ; Get command 138 | mov A, Paral 139 | mov Cmdl, A 140 | mov A, Parah 141 | mov Cmdh, A 142 | clr C 143 | mov A, Cmdh 144 | subb A, #0FEh 145 | jc mai2 ; Jump if not set address or set buffer 146 | 147 | acall getw ; Address or number of bytes 148 | mov Byte_Cntl, Paral ; Store number of bytes for set buffer 149 | mov Byte_Cnth, Parah 150 | mov A, Cmdh 151 | jnb ACC.0, mai2 ; Jump if set buffer 152 | 153 | mov DPL, Paral ; Store flash address (for set address) 154 | mov DPH, Parah 155 | 156 | mai2:acall getw ; Get CRC 157 | mov Paral, #ERRORCRC 158 | jnz mai1 159 | clr C 160 | mov A, Cmdh 161 | subb A, #0FEh 162 | jz setbuf ; If command is set buffer, receive data 163 | jnc main 164 | 165 | cjne Cmdh, #0, mai4 ; Jump if command != 0 (and not set buffer) 166 | 167 | ; Run application/restart bootloader 168 | mov A, Cmdl 169 | jz rst 170 | exit:mov Bit_Access, #0 ; Clear variable used by flash lock detect 171 | mov Bit_Access_Int, #0FFh ; Set variable to indicate that program execution came from bootloader 172 | mov BL_Flash_Key_1, #0 ; Set flash keys to invalid values 173 | mov BL_Flash_Key_2, #0 174 | ljmp 0000h 175 | rst: ajmp init 176 | 177 | ; Set buffer 178 | setbuf:mov Xl, Byte_Cntl ; Set number of bytes 179 | mov Xh, Byte_Cnth 180 | inc Xl 181 | inc Xh 182 | set4:djnz Xl, set5 183 | djnz Xh, set5 184 | ajmp set6 185 | 186 | set5:acall getc ; Receive data 187 | mov A, Paral 188 | movx @Xl, A ; Store data in XRAM 189 | ajmp set4 190 | 191 | set6:inc Cmdh 192 | ajmp mai2 193 | 194 | mai4:clr C 195 | mov A, Cmdh 196 | subb A, #3 197 | jnc mai5 ; Jump if command >= 3 198 | 199 | ; Program/erase 200 | mov A, Cmdh 201 | mov C, ACC.0 202 | mov Bit_Reg.0, C 203 | mov Paral, #ERRORPROG 204 | clr C 205 | mov A, DPL 206 | subb A, #low(BOOT_START) 207 | mov A, DPH 208 | subb A, #high(BOOT_START) 209 | jnc mai1 ; Jump if in bootloader segment 210 | jb Bit_Reg.0, pro3 ; Jump if program command 211 | 212 | ; Erase flash 213 | orl PSCTL, #02h ; Set the PSEE bit 214 | orl PSCTL, #01h ; Set the PSWE bit 215 | mov FLKEY, BL_Flash_Key_1 ; First key code 216 | mov FLKEY, BL_Flash_Key_2 ; Second key code 217 | movx @DPTR, A 218 | jnb Bit_Reg.0, pro6 ; Jump if erase command 219 | 220 | ; Program flash 221 | pro3:mov Xl, Byte_Cntl ; Set number of bytes 222 | mov Xh, Byte_Cnth 223 | inc Xl 224 | inc Xh 225 | orl PSCTL, #01h ; Set the PSWE bit 226 | anl PSCTL, #0FDh ; Clear the PSEE bit 227 | pro4:djnz Xl, pro5 228 | djnz Xh, pro5 229 | ajmp pro6 230 | 231 | pro5: 232 | clr C 233 | mov A, DPH ; Check that address is not in bootloader area 234 | subb A, #1Ch 235 | jc ($+5) 236 | 237 | inc DPTR ; Increment flash address 238 | ajmp pro4 239 | 240 | movx A, @Xl ; Read from XRAM 241 | mov FLKEY, BL_Flash_Key_1 ; First key code 242 | mov FLKEY, BL_Flash_Key_2 ; Second key code 243 | movx @DPTR, A ; Write to flash 244 | inc DPTR ; Increment flash address 245 | ajmp pro4 246 | 247 | pro6:anl PSCTL, #0FCh ; Clear the PSEE and PSWE bits 248 | ajmp main ; Successfully done erase or program 249 | 250 | ; Read flash 251 | mai5:mov Paral, #ERRORCOMMAND ; Illegal command 252 | cjne Cmdh, #3, mai6 ; Jump if not read flash command 253 | 254 | rd1: clr A 255 | movc A, @A+DPTR ; Read from flash 256 | inc DPTR ; Increment flash address 257 | mov Paral, A 258 | acall putp 259 | djnz Cmdl, rd1 ; Decrement bytes to read 260 | 261 | acall putw ; CRC 262 | ajmp main 263 | 264 | mai6:ajmp mai1 265 | 266 | 267 | 268 | 269 | ; Send char with crc 270 | putw:mov Paral, Crcl 271 | mov Parah, Crch 272 | acall putc 273 | mov A, Parah 274 | mov Paral, A 275 | putp:mov A, Paral 276 | xrl Crcl, A 277 | mov Bit_Cnt, #8 278 | put1:clr C 279 | mov A, Crch 280 | rrc A 281 | mov Crch, A 282 | mov A, Crcl 283 | rrc A 284 | mov Crcl, A 285 | jnc put2 286 | 287 | xrl Crch, #high(POLYNOM) 288 | xrl Crcl, #low(POLYNOM) 289 | 290 | put2:djnz Bit_Cnt, put1 291 | 292 | 293 | ; Send char 294 | putc:acall waitf 295 | acall waitf 296 | mov Bit_Cnt, #10 297 | mov A, Paral 298 | cpl A 299 | put3:jb Bit_Reg.1, ($+5) 300 | setb RTX_PORT.RTX_PIN ; Set pin high 301 | jnb Bit_Reg.1, ($+5) 302 | clr RTX_PORT.RTX_PIN ; Set pin low 303 | acall waitf 304 | clr C 305 | rrc A 306 | jc put4 307 | 308 | clr Bit_Reg.1 309 | 310 | put4:djnz Bit_Cnt, put3 311 | 312 | ret 313 | 314 | 315 | ; Receive char/word 316 | getw:acall getc 317 | mov A, Paral 318 | mov Parah, A 319 | getc:jb RTX_PORT.RTX_PIN, ($+5) ; Wait for high 320 | ajmp getc 321 | 322 | get1:jnb RTX_PORT.RTX_PIN, ($+5) ; Wait for low 323 | ajmp get1 324 | 325 | getx:mov Bit_Cnt, #8 326 | mov Cntl, Baudl 327 | mov Cnth, Baudh 328 | clr C 329 | mov A, Cnth ; Wait half a baud 330 | rrc A 331 | mov Cnth, A 332 | mov A, Cntl 333 | rrc A 334 | mov Cntl, A 335 | acall waith 336 | get2:acall waitf ; Wait one baud 337 | clr C 338 | mov A, Paral 339 | rrc A 340 | jnb RTX_PORT.RTX_PIN, ($+5) 341 | orl A, #080h 342 | 343 | mov Paral, A 344 | jnb ACC.7, ($+6) 345 | xrl Crcl, #low(POLYNOM) 346 | 347 | clr C 348 | mov A, Crch 349 | rrc A 350 | mov Crch, A 351 | mov A, Crcl 352 | rrc A 353 | mov Crcl, A 354 | jnc get3 355 | 356 | xrl Crch, #high(POLYNOM) 357 | xrl Crcl, #low(POLYNOM) 358 | 359 | get3:djnz Bit_Cnt, get2 360 | 361 | mov A, Crcl 362 | xrl A, Crch 363 | xrl A, Crch 364 | mov Crcl, A 365 | ret 366 | 367 | 368 | ; UART delays 369 | waitf:mov Cntl, Baudl 370 | mov Cnth, Baudh 371 | waith:inc Cntl 372 | inc Cnth 373 | wait1:djnz Cntl, wait1 374 | djnz Cnth, wait1 375 | 376 | setb Bit_Reg.1 377 | ret 378 | 379 | 380 | BOOT_SIGN: DB "BLHeli" 381 | 382 | BOOT_MSG: DB "471d" ; Interface-MCU_BootlaoderRevision 383 | 384 | BOOT_INFO: DB SIGNATURE_001, SIGNATURE_002, BOOT_VERSION, BOOT_PAGES 385 | 386 | -------------------------------------------------------------------------------- /src/BLHeliPgm.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; BLHeliTxPgm SiLabs 26 | ; 27 | ; EEPROM is not available in SiLabs MCUs 28 | ; Therefore a segment of the flash is used as "EEPROM" 29 | ; 30 | ;**** **** **** **** **** 31 | 32 | 33 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 34 | ; 35 | ; Read all eeprom parameters routine 36 | ; 37 | ; No assumptions 38 | ; 39 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 40 | read_all_eeprom_parameters: 41 | ; Check initialized signature 42 | mov DPTR, #Eep_Initialized_L 43 | mov Temp1, #Bit_Access 44 | call read_eeprom_byte 45 | mov A, Bit_Access 46 | cjne A, #055h, read_eeprom_store_defaults 47 | inc DPTR ; Now Eep_Initialized_H 48 | call read_eeprom_byte 49 | mov A, Bit_Access 50 | cjne A, #0AAh, read_eeprom_store_defaults 51 | jmp read_eeprom_read 52 | 53 | 54 | read_eeprom_store_defaults: 55 | mov Flash_Key_1, #0A5h 56 | mov Flash_Key_2, #0F1h 57 | call set_default_parameters 58 | call erase_and_store_all_in_eeprom 59 | mov Flash_Key_1, #0 60 | mov Flash_Key_2, #0 61 | jmp read_eeprom_exit 62 | 63 | read_eeprom_read: 64 | ; Read eeprom 65 | mov DPTR, #_Eep_Pgm_Gov_P_Gain 66 | mov Temp1, #_Pgm_Gov_P_Gain 67 | mov Temp4, #10 68 | read_eeprom_block1: 69 | call read_eeprom_byte 70 | inc DPTR 71 | inc Temp1 72 | djnz Temp4, read_eeprom_block1 73 | 74 | mov DPTR, #Eep_Enable_TX_Program 75 | mov Temp1, #Pgm_Enable_TX_Program 76 | mov Temp4, #26 ; 26 parameters 77 | read_eeprom_block2: 78 | call read_eeprom_byte 79 | inc DPTR 80 | inc Temp1 81 | djnz Temp4, read_eeprom_block2 82 | 83 | mov DPTR, #Eep_Dummy ; Set pointer to uncritical area 84 | 85 | read_eeprom_exit: 86 | ret 87 | 88 | 89 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 90 | ; 91 | ; Erase flash and store all parameter value in EEPROM routine 92 | ; 93 | ; No assumptions 94 | ; 95 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 96 | erase_and_store_all_in_eeprom: 97 | clr IE_EA ; Disable interrupts 98 | call read_tags 99 | call erase_flash ; Erase flash 100 | 101 | mov DPTR, #Eep_FW_Main_Revision ; Store firmware main revision 102 | mov A, #EEPROM_FW_MAIN_REVISION 103 | call write_eeprom_byte_from_acc 104 | 105 | inc DPTR ; Now firmware sub revision 106 | mov A, #EEPROM_FW_SUB_REVISION 107 | call write_eeprom_byte_from_acc 108 | 109 | inc DPTR ; Now layout revision 110 | mov A, #EEPROM_LAYOUT_REVISION 111 | call write_eeprom_byte_from_acc 112 | 113 | ; Write eeprom 114 | mov DPTR, #_Eep_Pgm_Gov_P_Gain 115 | mov Temp1, #_Pgm_Gov_P_Gain 116 | mov Temp4, #10 117 | write_eeprom_block1: 118 | call write_eeprom_byte 119 | inc DPTR 120 | inc Temp1 121 | djnz Temp4, write_eeprom_block1 122 | 123 | mov DPTR, #Eep_Enable_TX_Program 124 | mov Temp1, #Pgm_Enable_TX_Program 125 | mov Temp4, #26 ; 26 parameters 126 | write_eeprom_block2: 127 | call write_eeprom_byte 128 | inc DPTR 129 | inc Temp1 130 | djnz Temp4, write_eeprom_block2 131 | 132 | call write_tags 133 | call write_eeprom_signature 134 | mov DPTR, #Eep_Dummy ; Set pointer to uncritical area 135 | ret 136 | 137 | 138 | 139 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 140 | ; 141 | ; Read eeprom byte routine 142 | ; 143 | ; Gives data in A and in address given by Temp1. Assumes address in DPTR 144 | ; Also assumes address high byte to be zero 145 | ; 146 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 147 | read_eeprom_byte: 148 | clr A 149 | movc A, @A+DPTR ; Read from flash 150 | mov @Temp1, A 151 | ret 152 | 153 | 154 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 155 | ; 156 | ; Write eeprom byte routine 157 | ; 158 | ; Assumes data in address given by Temp1, or in accumulator. Assumes address in DPTR 159 | ; Also assumes address high byte to be zero 160 | ; 161 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 162 | write_eeprom_byte: 163 | mov A, @Temp1 164 | write_eeprom_byte_from_acc: 165 | orl PSCTL, #01h ; Set the PSWE bit 166 | anl PSCTL, #0FDh ; Clear the PSEE bit 167 | mov Temp8, A 168 | clr C 169 | mov A, DPH ; Check that address is not in bootloader area 170 | subb A, #1Ch 171 | jc ($+3) 172 | 173 | ret 174 | 175 | mov A, Temp8 176 | mov FLKEY, Flash_Key_1 ; First key code 177 | mov FLKEY, Flash_Key_2 ; Second key code 178 | movx @DPTR, A ; Write to flash 179 | anl PSCTL, #0FEh ; Clear the PSWE bit 180 | ret 181 | 182 | 183 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 184 | ; 185 | ; Erase flash routine (erases the flash segment used for "eeprom" variables) 186 | ; 187 | ; No assumptions 188 | ; 189 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 190 | erase_flash: 191 | orl PSCTL, #02h ; Set the PSEE bit 192 | orl PSCTL, #01h ; Set the PSWE bit 193 | mov FLKEY, Flash_Key_1 ; First key code 194 | mov FLKEY, Flash_Key_2 ; Second key code 195 | mov DPTR, #Eep_Initialized_L 196 | movx @DPTR, A 197 | anl PSCTL, #0FCh ; Clear the PSEE and PSWE bits 198 | ret 199 | 200 | 201 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 202 | ; 203 | ; Write eeprom signature routine 204 | ; 205 | ; No assumptions 206 | ; 207 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 208 | write_eeprom_signature: 209 | mov DPTR, #Eep_Initialized_L 210 | mov A, #055h 211 | call write_eeprom_byte_from_acc 212 | 213 | mov DPTR, #Eep_Initialized_H 214 | mov A, #0AAh 215 | call write_eeprom_byte_from_acc 216 | ret 217 | 218 | 219 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 220 | ; 221 | ; Read all tags from flash and store in temporary storage 222 | ; 223 | ; No assumptions 224 | ; 225 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 226 | read_tags: 227 | mov Temp3, #48 ; Number of tags 228 | mov Temp2, #Temp_Storage ; Set RAM address 229 | mov Temp1, #Bit_Access 230 | mov DPTR, #Eep_ESC_Layout ; Set flash address 231 | read_tag: 232 | call read_eeprom_byte 233 | mov A, Bit_Access 234 | mov @Temp2, A ; Write to RAM 235 | inc Temp2 236 | inc DPTR 237 | djnz Temp3, read_tag 238 | ret 239 | 240 | 241 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 242 | ; 243 | ; Write all tags from temporary storage and store in flash 244 | ; 245 | ; No assumptions 246 | ; 247 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 248 | write_tags: 249 | mov Temp3, #48 ; Number of tags 250 | mov Temp2, #Temp_Storage ; Set RAM address 251 | mov DPTR, #Eep_ESC_Layout ; Set flash address 252 | write_tag: 253 | mov A, @Temp2 ; Read from RAM 254 | call write_eeprom_byte_from_acc 255 | inc Temp2 256 | inc DPTR 257 | djnz Temp3, write_tag 258 | ret 259 | 260 | 261 | ;**;**** **** **** **** **** **** **** **** **** **** **** **** **** 262 | ; 263 | ; Wait 1 second routine 264 | ; 265 | ; No assumptions 266 | ; 267 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 268 | wait1s: 269 | mov Temp5, #5 270 | wait1s_loop: 271 | call wait200ms 272 | djnz Temp5, wait1s_loop 273 | ret 274 | 275 | 276 | ;**;**** **** **** **** **** **** **** **** **** **** **** **** **** 277 | ; 278 | ; Success beep routine 279 | ; 280 | ; No assumptions 281 | ; 282 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 283 | success_beep: 284 | clr IE_EA ; Disable all interrupts 285 | call beep_f1 286 | call beep_f2 287 | call beep_f3 288 | call beep_f4 289 | call wait10ms 290 | call beep_f1 291 | call beep_f2 292 | call beep_f3 293 | call beep_f4 294 | call wait10ms 295 | call beep_f1 296 | call beep_f2 297 | call beep_f3 298 | call beep_f4 299 | setb IE_EA ; Enable all interrupts 300 | ret 301 | 302 | 303 | ;**;**** **** **** **** **** **** **** **** **** **** **** **** **** 304 | ; 305 | ; Success beep inverted routine 306 | ; 307 | ; No assumptions 308 | ; 309 | ;**** **** **** **** **** **** **** **** **** **** **** **** **** 310 | success_beep_inverted: 311 | clr IE_EA ; Disable all interrupts 312 | call beep_f4 313 | call beep_f3 314 | call beep_f2 315 | call beep_f1 316 | call wait10ms 317 | call beep_f4 318 | call beep_f3 319 | call beep_f2 320 | call beep_f1 321 | call wait10ms 322 | call beep_f4 323 | call beep_f3 324 | call beep_f2 325 | call beep_f1 326 | setb IE_EA ; Enable all interrupts 327 | ret 328 | 329 | 330 | -------------------------------------------------------------------------------- /src/BLHeli_S.asm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/betaflight/BLHeli_S/5df60609e2808709dd868781c809f0b3b9e66f61/src/BLHeli_S.asm -------------------------------------------------------------------------------- /src/D.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "D". Com fets are active low for H/L_N driver and EN_N/PWM driver 26 | ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#D_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#D_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#D_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#D_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#D_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#D_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#D_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#D_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#D_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#D_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#D_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#D_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#D_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#D_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#D_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#D_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#D_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#D_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#D_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#D_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#D_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#D_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Comp_Com EQU 3 ;i 134 | Mux_A EQU 2 ;i 135 | Mux_C EQU 1 ;i 136 | Mux_B EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #00h ; Damping noninverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | 316 | ;********************* 317 | ; PORT 1 definitions * 318 | ;********************* 319 | ; EQU 7 ;i 320 | ; EQU 6 ;i 321 | CcomFET EQU 5 ;o 322 | CpwmFET EQU 4 ;o 323 | BcomFET EQU 3 ;o 324 | BpwmFET EQU 2 ;o 325 | AcomFET EQU 1 ;o 326 | ApwmFET EQU 0 ;o 327 | 328 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 329 | P1_INIT EQU (1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 330 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_SKIP EQU 3Fh 332 | 333 | ApwmFET_on MACRO 334 | setb P1.ApwmFET 335 | IF FETON_DELAY == 0 336 | clr P1.AcomFET 337 | ENDIF 338 | ENDM 339 | ApwmFET_off MACRO 340 | IF FETON_DELAY != 0 341 | clr P1.ApwmFET 342 | ELSE 343 | setb P1.AcomFET 344 | ENDIF 345 | ENDM 346 | BpwmFET_on MACRO 347 | setb P1.BpwmFET 348 | IF FETON_DELAY == 0 349 | clr P1.BcomFET 350 | ENDIF 351 | ENDM 352 | BpwmFET_off MACRO 353 | IF FETON_DELAY != 0 354 | clr P1.BpwmFET 355 | ELSE 356 | setb P1.BcomFET 357 | ENDIF 358 | ENDM 359 | CpwmFET_on MACRO 360 | setb P1.CpwmFET 361 | IF FETON_DELAY == 0 362 | clr P1.CcomFET 363 | ENDIF 364 | ENDM 365 | CpwmFET_off MACRO 366 | IF FETON_DELAY != 0 367 | clr P1.CpwmFET 368 | ELSE 369 | setb P1.CcomFET 370 | ENDIF 371 | ENDM 372 | All_pwmFETs_Off MACRO 373 | IF FETON_DELAY != 0 374 | clr P1.ApwmFET 375 | clr P1.BpwmFET 376 | clr P1.CpwmFET 377 | ELSE 378 | setb P1.AcomFET 379 | setb P1.BcomFET 380 | setb P1.CcomFET 381 | ENDIF 382 | ENDM 383 | 384 | AcomFET_on MACRO 385 | IF FETON_DELAY == 0 386 | clr P1.ApwmFET 387 | ENDIF 388 | clr P1.AcomFET 389 | ENDM 390 | AcomFET_off MACRO 391 | setb P1.AcomFET 392 | ENDM 393 | BcomFET_on MACRO 394 | IF FETON_DELAY == 0 395 | clr P1.BpwmFET 396 | ENDIF 397 | clr P1.BcomFET 398 | ENDM 399 | BcomFET_off MACRO 400 | setb P1.BcomFET 401 | ENDM 402 | CcomFET_on MACRO 403 | IF FETON_DELAY == 0 404 | clr P1.CpwmFET 405 | ENDIF 406 | clr P1.CcomFET 407 | ENDM 408 | CcomFET_off MACRO 409 | setb P1.CcomFET 410 | ENDM 411 | All_comFETs_Off MACRO 412 | setb P1.AcomFET 413 | setb P1.BcomFET 414 | setb P1.CcomFET 415 | ENDM 416 | 417 | Set_Pwm_A MACRO 418 | IF FETON_DELAY == 0 419 | clr P1.AcomFET 420 | mov P1SKIP, #3Eh 421 | ELSE 422 | mov P1SKIP, #3Ch 423 | ENDIF 424 | ENDM 425 | Set_Pwm_B MACRO 426 | IF FETON_DELAY == 0 427 | clr P1.BcomFET 428 | mov P1SKIP, #3Bh 429 | ELSE 430 | mov P1SKIP, #33h 431 | ENDIF 432 | ENDM 433 | Set_Pwm_C MACRO 434 | IF FETON_DELAY == 0 435 | clr P1.CcomFET 436 | mov P1SKIP, #2Fh 437 | ELSE 438 | mov P1SKIP, #0Fh 439 | ENDIF 440 | ENDM 441 | Set_Pwms_Off MACRO 442 | mov P1SKIP, #3Fh 443 | ENDM 444 | 445 | Set_Comp_Phase_A MACRO 446 | mov CMP0MX, #23h ; Set comparator multiplexer to phase A 447 | ENDM 448 | Set_Comp_Phase_B MACRO 449 | mov CMP0MX, #03h ; Set comparator multiplexer to phase B 450 | ENDM 451 | Set_Comp_Phase_C MACRO 452 | mov CMP0MX, #13h ; Set comparator multiplexer to phase C 453 | ENDM 454 | Read_Comp_Out MACRO 455 | mov A, CMP0CN0 ; Read comparator output 456 | ENDM 457 | 458 | 459 | ;********************* 460 | ; PORT 2 definitions * 461 | ;********************* 462 | DebugPin EQU 0 ;o 463 | 464 | P2_PUSHPULL EQU (1 SHL DebugPin) 465 | 466 | 467 | ;********************** 468 | ; MCU specific macros * 469 | ;********************** 470 | Interrupt_Table_Definition MACRO 471 | CSEG AT 0 ; Code segment start 472 | jmp reset 473 | CSEG AT 03h ; Int0 interrupt 474 | jmp int0_int 475 | IF MCU_48MHZ == 1 476 | CSEG AT 0Bh ; Timer0 overflow interrupt 477 | jmp t0_int 478 | ENDIF 479 | CSEG AT 13h ; Int1 interrupt 480 | jmp int1_int 481 | CSEG AT 1Bh ; Timer1 overflow interrupt 482 | jmp t1_int 483 | CSEG AT 2Bh ; Timer2 overflow interrupt 484 | jmp t2_int 485 | CSEG AT 5Bh ; Pca interrupt 486 | jmp pca_int 487 | CSEG AT 73h ; Timer3 overflow/compare interrupt 488 | jmp t3_int 489 | ENDM 490 | 491 | Initialize_Xbar MACRO 492 | mov XBR2, #40h ; Xbar enabled 493 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 494 | ENDM 495 | 496 | Initialize_Comparator MACRO 497 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 498 | mov CMP0MD, #00h ; Comparator response time 100ns 499 | ENDM 500 | Initialize_Adc MACRO 501 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 502 | IF MCU_48MHZ == 0 503 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 504 | ELSE 505 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 506 | ENDIF 507 | mov ADC0MX, #10h ; Select temp sensor input 508 | mov ADC0CN0, #80h ; ADC enabled 509 | mov ADC0CN1, #01h ; Common mode buffer enabled 510 | ENDM 511 | Start_Adc MACRO 512 | mov ADC0CN0, #90h ; ADC start 513 | ENDM 514 | Read_Adc_Result MACRO 515 | mov Temp1, ADC0L 516 | mov Temp2, ADC0H 517 | ENDM 518 | Stop_Adc MACRO 519 | ENDM 520 | Set_RPM_Out MACRO 521 | ENDM 522 | Clear_RPM_Out MACRO 523 | ENDM 524 | Set_MCU_Clk_24MHz MACRO 525 | mov CLKSEL, #13h ; Set clock to 24MHz 526 | mov SFRPAGE, #10h 527 | mov PFE0CN, #00h ; Set flash timing for 24MHz 528 | mov SFRPAGE, #00h 529 | mov Clock_Set_At_48MHz, #0 530 | ENDM 531 | Set_MCU_Clk_48MHz MACRO 532 | mov SFRPAGE, #10h 533 | mov PFE0CN, #30h ; Set flash timing for 48MHz 534 | mov SFRPAGE, #00h 535 | mov CLKSEL, #03h ; Set clock to 48MHz 536 | mov Clock_Set_At_48MHz, #1 537 | ENDM 538 | Set_LED_0 MACRO 539 | ENDM 540 | Clear_LED_0 MACRO 541 | ENDM 542 | Set_LED_1 MACRO 543 | ENDM 544 | Clear_LED_1 MACRO 545 | ENDM 546 | Set_LED_2 MACRO 547 | ENDM 548 | Clear_LED_2 MACRO 549 | ENDM 550 | Set_LED_3 MACRO 551 | ENDM 552 | Clear_LED_3 MACRO 553 | ENDM 554 | -------------------------------------------------------------------------------- /src/F.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "F". Equals "A", but with Mux_A and Mux_C swapped 26 | ; X X RC X MA MB MC CC X X Cc Cp Bc Bp Ac Ap 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#F_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#F_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#F_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#F_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#F_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#F_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#F_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#F_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#F_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#F_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#F_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#F_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#F_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#F_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#F_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#F_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#F_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#F_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#F_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#F_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#F_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#F_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Mux_A EQU 3 ;i 134 | Mux_B EQU 2 ;i 135 | Mux_C EQU 1 ;i 136 | Comp_Com EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | 316 | ;********************* 317 | ; PORT 1 definitions * 318 | ;********************* 319 | ; EQU 7 ;i 320 | ; EQU 6 ;i 321 | CcomFET EQU 5 ;o 322 | CpwmFET EQU 4 ;o 323 | BcomFET EQU 3 ;o 324 | BpwmFET EQU 2 ;o 325 | AcomFET EQU 1 ;o 326 | ApwmFET EQU 0 ;o 327 | 328 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 329 | P1_INIT EQU 00h 330 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_SKIP EQU 3Fh 332 | 333 | ApwmFET_on MACRO 334 | setb P1.ApwmFET 335 | IF FETON_DELAY == 0 336 | setb P1.AcomFET 337 | ENDIF 338 | ENDM 339 | ApwmFET_off MACRO 340 | IF FETON_DELAY != 0 341 | clr P1.ApwmFET 342 | ELSE 343 | clr P1.AcomFET 344 | ENDIF 345 | ENDM 346 | BpwmFET_on MACRO 347 | setb P1.BpwmFET 348 | IF FETON_DELAY == 0 349 | setb P1.BcomFET 350 | ENDIF 351 | ENDM 352 | BpwmFET_off MACRO 353 | IF FETON_DELAY != 0 354 | clr P1.BpwmFET 355 | ELSE 356 | clr P1.BcomFET 357 | ENDIF 358 | ENDM 359 | CpwmFET_on MACRO 360 | setb P1.CpwmFET 361 | IF FETON_DELAY == 0 362 | setb P1.CcomFET 363 | ENDIF 364 | ENDM 365 | CpwmFET_off MACRO 366 | IF FETON_DELAY != 0 367 | clr P1.CpwmFET 368 | ELSE 369 | clr P1.CcomFET 370 | ENDIF 371 | ENDM 372 | All_pwmFETs_Off MACRO 373 | IF FETON_DELAY != 0 374 | clr P1.ApwmFET 375 | clr P1.BpwmFET 376 | clr P1.CpwmFET 377 | ELSE 378 | clr P1.AcomFET 379 | clr P1.BcomFET 380 | clr P1.CcomFET 381 | ENDIF 382 | ENDM 383 | 384 | AcomFET_on MACRO 385 | IF FETON_DELAY == 0 386 | clr P1.ApwmFET 387 | ENDIF 388 | setb P1.AcomFET 389 | ENDM 390 | AcomFET_off MACRO 391 | clr P1.AcomFET 392 | ENDM 393 | BcomFET_on MACRO 394 | IF FETON_DELAY == 0 395 | clr P1.BpwmFET 396 | ENDIF 397 | setb P1.BcomFET 398 | ENDM 399 | BcomFET_off MACRO 400 | clr P1.BcomFET 401 | ENDM 402 | CcomFET_on MACRO 403 | IF FETON_DELAY == 0 404 | clr P1.CpwmFET 405 | ENDIF 406 | setb P1.CcomFET 407 | ENDM 408 | CcomFET_off MACRO 409 | clr P1.CcomFET 410 | ENDM 411 | All_comFETs_Off MACRO 412 | clr P1.AcomFET 413 | clr P1.BcomFET 414 | clr P1.CcomFET 415 | ENDM 416 | 417 | Set_Pwm_A MACRO 418 | IF FETON_DELAY == 0 419 | setb P1.AcomFET 420 | mov P1SKIP, #3Eh 421 | ELSE 422 | mov P1SKIP, #3Ch 423 | ENDIF 424 | ENDM 425 | Set_Pwm_B MACRO 426 | IF FETON_DELAY == 0 427 | setb P1.BcomFET 428 | mov P1SKIP, #3Bh 429 | ELSE 430 | mov P1SKIP, #33h 431 | ENDIF 432 | ENDM 433 | Set_Pwm_C MACRO 434 | IF FETON_DELAY == 0 435 | setb P1.CcomFET 436 | mov P1SKIP, #2Fh 437 | ELSE 438 | mov P1SKIP, #0Fh 439 | ENDIF 440 | ENDM 441 | Set_Pwms_Off MACRO 442 | mov P1SKIP, #3Fh 443 | ENDM 444 | 445 | Set_Comp_Phase_A MACRO 446 | mov CMP0MX, #30h ; Set comparator multiplexer to phase A 447 | ENDM 448 | Set_Comp_Phase_B MACRO 449 | mov CMP0MX, #20h ; Set comparator multiplexer to phase B 450 | ENDM 451 | Set_Comp_Phase_C MACRO 452 | mov CMP0MX, #10h ; Set comparator multiplexer to phase C 453 | ENDM 454 | Read_Comp_Out MACRO 455 | mov A, CMP0CN0 ; Read comparator output 456 | ENDM 457 | 458 | 459 | ;********************* 460 | ; PORT 2 definitions * 461 | ;********************* 462 | DebugPin EQU 0 ;o 463 | 464 | P2_PUSHPULL EQU (1 SHL DebugPin) 465 | 466 | 467 | ;********************** 468 | ; MCU specific macros * 469 | ;********************** 470 | Interrupt_Table_Definition MACRO 471 | CSEG AT 0 ; Code segment start 472 | jmp reset 473 | CSEG AT 03h ; Int0 interrupt 474 | jmp int0_int 475 | IF MCU_48MHZ == 1 476 | CSEG AT 0Bh ; Timer0 overflow interrupt 477 | jmp t0_int 478 | ENDIF 479 | CSEG AT 13h ; Int1 interrupt 480 | jmp int1_int 481 | CSEG AT 1Bh ; Timer1 overflow interrupt 482 | jmp t1_int 483 | CSEG AT 2Bh ; Timer2 overflow interrupt 484 | jmp t2_int 485 | CSEG AT 5Bh ; Pca interrupt 486 | jmp pca_int 487 | CSEG AT 73h ; Timer3 overflow/compare interrupt 488 | jmp t3_int 489 | ENDM 490 | 491 | Initialize_Xbar MACRO 492 | mov XBR2, #40h ; Xbar enabled 493 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 494 | ENDM 495 | 496 | Initialize_Comparator MACRO 497 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 498 | mov CMP0MD, #00h ; Comparator response time 100ns 499 | ENDM 500 | Initialize_Adc MACRO 501 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 502 | IF MCU_48MHZ == 0 503 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 504 | ELSE 505 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 506 | ENDIF 507 | mov ADC0MX, #10h ; Select temp sensor input 508 | mov ADC0CN0, #80h ; ADC enabled 509 | mov ADC0CN1, #01h ; Common mode buffer enabled 510 | ENDM 511 | Start_Adc MACRO 512 | mov ADC0CN0, #90h ; ADC start 513 | ENDM 514 | Read_Adc_Result MACRO 515 | mov Temp1, ADC0L 516 | mov Temp2, ADC0H 517 | ENDM 518 | Stop_Adc MACRO 519 | ENDM 520 | Set_RPM_Out MACRO 521 | ENDM 522 | Clear_RPM_Out MACRO 523 | ENDM 524 | Set_MCU_Clk_24MHz MACRO 525 | mov CLKSEL, #13h ; Set clock to 24MHz 526 | mov SFRPAGE, #10h 527 | mov PFE0CN, #00h ; Set flash timing for 24MHz 528 | mov SFRPAGE, #00h 529 | mov Clock_Set_At_48MHz, #0 530 | ENDM 531 | Set_MCU_Clk_48MHz MACRO 532 | mov SFRPAGE, #10h 533 | mov PFE0CN, #30h ; Set flash timing for 48MHz 534 | mov SFRPAGE, #00h 535 | mov CLKSEL, #03h ; Set clock to 48MHz 536 | mov Clock_Set_At_48MHz, #1 537 | ENDM 538 | Set_LED_0 MACRO 539 | ENDM 540 | Clear_LED_0 MACRO 541 | ENDM 542 | Set_LED_1 MACRO 543 | ENDM 544 | Clear_LED_1 MACRO 545 | ENDM 546 | Set_LED_2 MACRO 547 | ENDM 548 | Clear_LED_2 MACRO 549 | ENDM 550 | Set_LED_3 MACRO 551 | ENDM 552 | Clear_LED_3 MACRO 553 | ENDM 554 | -------------------------------------------------------------------------------- /src/G.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "G" 26 | ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#G_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#G_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#G_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#G_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#G_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#G_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#G_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#G_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#G_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#G_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#G_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#G_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#G_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#G_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#G_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#G_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#G_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#G_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#G_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#G_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#G_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#G_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Comp_Com EQU 3 ;i 134 | Mux_A EQU 2 ;i 135 | Mux_C EQU 1 ;i 136 | Mux_B EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | 316 | ;********************* 317 | ; PORT 1 definitions * 318 | ;********************* 319 | ; EQU 7 ;i 320 | ; EQU 6 ;i 321 | CcomFET EQU 5 ;o 322 | CpwmFET EQU 4 ;o 323 | BcomFET EQU 3 ;o 324 | BpwmFET EQU 2 ;o 325 | AcomFET EQU 1 ;o 326 | ApwmFET EQU 0 ;o 327 | 328 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 329 | P1_INIT EQU 00h 330 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_SKIP EQU 3Fh 332 | 333 | ApwmFET_on MACRO 334 | setb P1.ApwmFET 335 | IF FETON_DELAY == 0 336 | setb P1.AcomFET 337 | ENDIF 338 | ENDM 339 | ApwmFET_off MACRO 340 | IF FETON_DELAY != 0 341 | clr P1.ApwmFET 342 | ELSE 343 | clr P1.AcomFET 344 | ENDIF 345 | ENDM 346 | BpwmFET_on MACRO 347 | setb P1.BpwmFET 348 | IF FETON_DELAY == 0 349 | setb P1.BcomFET 350 | ENDIF 351 | ENDM 352 | BpwmFET_off MACRO 353 | IF FETON_DELAY != 0 354 | clr P1.BpwmFET 355 | ELSE 356 | clr P1.BcomFET 357 | ENDIF 358 | ENDM 359 | CpwmFET_on MACRO 360 | setb P1.CpwmFET 361 | IF FETON_DELAY == 0 362 | setb P1.CcomFET 363 | ENDIF 364 | ENDM 365 | CpwmFET_off MACRO 366 | IF FETON_DELAY != 0 367 | clr P1.CpwmFET 368 | ELSE 369 | clr P1.CcomFET 370 | ENDIF 371 | ENDM 372 | All_pwmFETs_Off MACRO 373 | IF FETON_DELAY != 0 374 | clr P1.ApwmFET 375 | clr P1.BpwmFET 376 | clr P1.CpwmFET 377 | ELSE 378 | clr P1.AcomFET 379 | clr P1.BcomFET 380 | clr P1.CcomFET 381 | ENDIF 382 | ENDM 383 | 384 | AcomFET_on MACRO 385 | IF FETON_DELAY == 0 386 | clr P1.ApwmFET 387 | ENDIF 388 | setb P1.AcomFET 389 | ENDM 390 | AcomFET_off MACRO 391 | clr P1.AcomFET 392 | ENDM 393 | BcomFET_on MACRO 394 | IF FETON_DELAY == 0 395 | clr P1.BpwmFET 396 | ENDIF 397 | setb P1.BcomFET 398 | ENDM 399 | BcomFET_off MACRO 400 | clr P1.BcomFET 401 | ENDM 402 | CcomFET_on MACRO 403 | IF FETON_DELAY == 0 404 | clr P1.CpwmFET 405 | ENDIF 406 | setb P1.CcomFET 407 | ENDM 408 | CcomFET_off MACRO 409 | clr P1.CcomFET 410 | ENDM 411 | All_comFETs_Off MACRO 412 | clr P1.AcomFET 413 | clr P1.BcomFET 414 | clr P1.CcomFET 415 | ENDM 416 | 417 | Set_Pwm_A MACRO 418 | IF FETON_DELAY == 0 419 | setb P1.AcomFET 420 | mov P1SKIP, #3Eh 421 | ELSE 422 | mov P1SKIP, #3Ch 423 | ENDIF 424 | ENDM 425 | Set_Pwm_B MACRO 426 | IF FETON_DELAY == 0 427 | setb P1.BcomFET 428 | mov P1SKIP, #3Bh 429 | ELSE 430 | mov P1SKIP, #33h 431 | ENDIF 432 | ENDM 433 | Set_Pwm_C MACRO 434 | IF FETON_DELAY == 0 435 | setb P1.CcomFET 436 | mov P1SKIP, #2Fh 437 | ELSE 438 | mov P1SKIP, #0Fh 439 | ENDIF 440 | ENDM 441 | Set_Pwms_Off MACRO 442 | mov P1SKIP, #3Fh 443 | ENDM 444 | 445 | Set_Comp_Phase_A MACRO 446 | mov CMP0MX, #23h ; Set comparator multiplexer to phase A 447 | ENDM 448 | Set_Comp_Phase_B MACRO 449 | mov CMP0MX, #03h ; Set comparator multiplexer to phase B 450 | ENDM 451 | Set_Comp_Phase_C MACRO 452 | mov CMP0MX, #13h ; Set comparator multiplexer to phase C 453 | ENDM 454 | Read_Comp_Out MACRO 455 | mov A, CMP0CN0 ; Read comparator output 456 | ENDM 457 | 458 | 459 | ;********************* 460 | ; PORT 2 definitions * 461 | ;********************* 462 | DebugPin EQU 0 ;o 463 | 464 | P2_PUSHPULL EQU (1 SHL DebugPin) 465 | 466 | 467 | ;********************** 468 | ; MCU specific macros * 469 | ;********************** 470 | Interrupt_Table_Definition MACRO 471 | CSEG AT 0 ; Code segment start 472 | jmp reset 473 | CSEG AT 03h ; Int0 interrupt 474 | jmp int0_int 475 | IF MCU_48MHZ == 1 476 | CSEG AT 0Bh ; Timer0 overflow interrupt 477 | jmp t0_int 478 | ENDIF 479 | CSEG AT 13h ; Int1 interrupt 480 | jmp int1_int 481 | CSEG AT 1Bh ; Timer1 overflow interrupt 482 | jmp t1_int 483 | CSEG AT 2Bh ; Timer2 overflow interrupt 484 | jmp t2_int 485 | CSEG AT 5Bh ; Pca interrupt 486 | jmp pca_int 487 | CSEG AT 73h ; Timer3 overflow/compare interrupt 488 | jmp t3_int 489 | ENDM 490 | 491 | Initialize_Xbar MACRO 492 | mov XBR2, #40h ; Xbar enabled 493 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 494 | ENDM 495 | 496 | Initialize_Comparator MACRO 497 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 498 | mov CMP0MD, #00h ; Comparator response time 100ns 499 | ENDM 500 | Initialize_Adc MACRO 501 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 502 | IF MCU_48MHZ == 0 503 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 504 | ELSE 505 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 506 | ENDIF 507 | mov ADC0MX, #10h ; Select temp sensor input 508 | mov ADC0CN0, #80h ; ADC enabled 509 | mov ADC0CN1, #01h ; Common mode buffer enabled 510 | ENDM 511 | Start_Adc MACRO 512 | mov ADC0CN0, #90h ; ADC start 513 | ENDM 514 | Read_Adc_Result MACRO 515 | mov Temp1, ADC0L 516 | mov Temp2, ADC0H 517 | ENDM 518 | Stop_Adc MACRO 519 | ENDM 520 | Set_RPM_Out MACRO 521 | ENDM 522 | Clear_RPM_Out MACRO 523 | ENDM 524 | Set_MCU_Clk_24MHz MACRO 525 | mov CLKSEL, #13h ; Set clock to 24MHz 526 | mov SFRPAGE, #10h 527 | mov PFE0CN, #00h ; Set flash timing for 24MHz 528 | mov SFRPAGE, #00h 529 | mov Clock_Set_At_48MHz, #0 530 | ENDM 531 | Set_MCU_Clk_48MHz MACRO 532 | mov SFRPAGE, #10h 533 | mov PFE0CN, #30h ; Set flash timing for 48MHz 534 | mov SFRPAGE, #00h 535 | mov CLKSEL, #03h ; Set clock to 48MHz 536 | mov Clock_Set_At_48MHz, #1 537 | ENDM 538 | Set_LED_0 MACRO 539 | ENDM 540 | Clear_LED_0 MACRO 541 | ENDM 542 | Set_LED_1 MACRO 543 | ENDM 544 | Clear_LED_1 MACRO 545 | ENDM 546 | Set_LED_2 MACRO 547 | ENDM 548 | Clear_LED_2 MACRO 549 | ENDM 550 | Set_LED_3 MACRO 551 | ENDM 552 | Clear_LED_3 MACRO 553 | ENDM 554 | -------------------------------------------------------------------------------- /src/I.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "I" 26 | ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#I_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#I_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#I_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#I_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#I_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#I_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#I_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#I_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#I_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#I_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#I_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#I_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#I_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#I_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#I_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#I_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#I_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#I_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#I_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#I_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#I_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#I_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Mux_C EQU 3 ;i 134 | Mux_B EQU 2 ;i 135 | Mux_A EQU 1 ;i 136 | Comp_Com EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | ;********************* 316 | ; PORT 1 definitions * 317 | ;********************* 318 | ; EQU 7 ;i 319 | ; EQU 6 ;i 320 | AcomFET EQU 5 ;o 321 | BcomFET EQU 4 ;o 322 | CcomFET EQU 3 ;o 323 | ApwmFET EQU 2 ;o 324 | BpwmFET EQU 1 ;o 325 | CpwmFET EQU 0 ;o 326 | 327 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 328 | P1_INIT EQU 00h 329 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 330 | P1_SKIP EQU 3Fh 331 | 332 | ApwmFET_on MACRO 333 | setb P1.ApwmFET 334 | IF FETON_DELAY == 0 335 | setb P1.AcomFET 336 | ENDIF 337 | ENDM 338 | ApwmFET_off MACRO 339 | IF FETON_DELAY != 0 340 | clr P1.ApwmFET 341 | ELSE 342 | clr P1.AcomFET 343 | ENDIF 344 | ENDM 345 | BpwmFET_on MACRO 346 | setb P1.BpwmFET 347 | IF FETON_DELAY == 0 348 | setb P1.BcomFET 349 | ENDIF 350 | ENDM 351 | BpwmFET_off MACRO 352 | IF FETON_DELAY != 0 353 | clr P1.BpwmFET 354 | ELSE 355 | clr P1.BcomFET 356 | ENDIF 357 | ENDM 358 | CpwmFET_on MACRO 359 | setb P1.CpwmFET 360 | IF FETON_DELAY == 0 361 | setb P1.CcomFET 362 | ENDIF 363 | ENDM 364 | CpwmFET_off MACRO 365 | IF FETON_DELAY != 0 366 | clr P1.CpwmFET 367 | ELSE 368 | clr P1.CcomFET 369 | ENDIF 370 | ENDM 371 | All_pwmFETs_Off MACRO 372 | IF FETON_DELAY != 0 373 | clr P1.ApwmFET 374 | clr P1.BpwmFET 375 | clr P1.CpwmFET 376 | ELSE 377 | clr P1.AcomFET 378 | clr P1.BcomFET 379 | clr P1.CcomFET 380 | ENDIF 381 | ENDM 382 | 383 | AcomFET_on MACRO 384 | IF FETON_DELAY == 0 385 | clr P1.ApwmFET 386 | ENDIF 387 | setb P1.AcomFET 388 | ENDM 389 | AcomFET_off MACRO 390 | clr P1.AcomFET 391 | ENDM 392 | BcomFET_on MACRO 393 | IF FETON_DELAY == 0 394 | clr P1.BpwmFET 395 | ENDIF 396 | setb P1.BcomFET 397 | ENDM 398 | BcomFET_off MACRO 399 | clr P1.BcomFET 400 | ENDM 401 | CcomFET_on MACRO 402 | IF FETON_DELAY == 0 403 | clr P1.CpwmFET 404 | ENDIF 405 | setb P1.CcomFET 406 | ENDM 407 | CcomFET_off MACRO 408 | clr P1.CcomFET 409 | ENDM 410 | All_comFETs_Off MACRO 411 | clr P1.AcomFET 412 | clr P1.BcomFET 413 | clr P1.CcomFET 414 | ENDM 415 | 416 | Set_Pwm_A MACRO 417 | IF FETON_DELAY == 0 418 | setb P1.AcomFET 419 | mov P1SKIP, #3Bh 420 | ELSE 421 | mov P1SKIP, #1Bh 422 | ENDIF 423 | ENDM 424 | Set_Pwm_B MACRO 425 | IF FETON_DELAY == 0 426 | setb P1.BcomFET 427 | mov P1SKIP, #3Dh 428 | ELSE 429 | mov P1SKIP, #2Dh 430 | ENDIF 431 | ENDM 432 | Set_Pwm_C MACRO 433 | IF FETON_DELAY == 0 434 | setb P1.CcomFET 435 | mov P1SKIP, #3Eh 436 | ELSE 437 | mov P1SKIP, #36h 438 | ENDIF 439 | ENDM 440 | Set_Pwms_Off MACRO 441 | mov P1SKIP, #7Fh 442 | ENDM 443 | 444 | Set_Comp_Phase_A MACRO 445 | mov CMP0MX, #10h ; Set comparator multiplexer to phase A 446 | ENDM 447 | Set_Comp_Phase_B MACRO 448 | mov CMP0MX, #20h ; Set comparator multiplexer to phase B 449 | ENDM 450 | Set_Comp_Phase_C MACRO 451 | mov CMP0MX, #30h ; Set comparator multiplexer to phase C 452 | ENDM 453 | Read_Comp_Out MACRO 454 | mov A, CMP0CN0 ; Read comparator output 455 | ENDM 456 | 457 | 458 | ;********************* 459 | ; PORT 2 definitions * 460 | ;********************* 461 | DebugPin EQU 0 ;o 462 | 463 | P2_PUSHPULL EQU (1 SHL DebugPin) 464 | 465 | 466 | ;********************** 467 | ; MCU specific macros * 468 | ;********************** 469 | Interrupt_Table_Definition MACRO 470 | CSEG AT 0 ; Code segment start 471 | jmp reset 472 | CSEG AT 03h ; Int0 interrupt 473 | jmp int0_int 474 | IF MCU_48MHZ == 1 475 | CSEG AT 0Bh ; Timer0 overflow interrupt 476 | jmp t0_int 477 | ENDIF 478 | CSEG AT 13h ; Int1 interrupt 479 | jmp int1_int 480 | CSEG AT 1Bh ; Timer1 overflow interrupt 481 | jmp t1_int 482 | CSEG AT 2Bh ; Timer2 overflow interrupt 483 | jmp t2_int 484 | CSEG AT 5Bh ; Pca interrupt 485 | jmp pca_int 486 | CSEG AT 73h ; Timer3 overflow/compare interrupt 487 | jmp t3_int 488 | ENDM 489 | 490 | Initialize_Xbar MACRO 491 | mov XBR2, #40h ; Xbar enabled 492 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 493 | ENDM 494 | 495 | Initialize_Comparator MACRO 496 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 497 | mov CMP0MD, #00h ; Comparator response time 100ns 498 | ENDM 499 | Initialize_Adc MACRO 500 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 501 | IF MCU_48MHZ == 0 502 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 503 | ELSE 504 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 505 | ENDIF 506 | mov ADC0MX, #10h ; Select temp sensor input 507 | mov ADC0CN0, #80h ; ADC enabled 508 | mov ADC0CN1, #01h ; Common mode buffer enabled 509 | ENDM 510 | Start_Adc MACRO 511 | mov ADC0CN0, #90h ; ADC start 512 | ENDM 513 | Read_Adc_Result MACRO 514 | mov Temp1, ADC0L 515 | mov Temp2, ADC0H 516 | ENDM 517 | Stop_Adc MACRO 518 | ENDM 519 | Set_RPM_Out MACRO 520 | ENDM 521 | Clear_RPM_Out MACRO 522 | ENDM 523 | Set_MCU_Clk_24MHz MACRO 524 | mov CLKSEL, #13h ; Set clock to 24MHz 525 | mov SFRPAGE, #10h 526 | mov PFE0CN, #00h ; Set flash timing for 24MHz 527 | mov SFRPAGE, #00h 528 | mov Clock_Set_At_48MHz, #0 529 | ENDM 530 | Set_MCU_Clk_48MHz MACRO 531 | mov SFRPAGE, #10h 532 | mov PFE0CN, #30h ; Set flash timing for 48MHz 533 | mov SFRPAGE, #00h 534 | mov CLKSEL, #03h ; Set clock to 48MHz 535 | mov Clock_Set_At_48MHz, #1 536 | ENDM 537 | Set_LED_0 MACRO 538 | ENDM 539 | Clear_LED_0 MACRO 540 | ENDM 541 | Set_LED_1 MACRO 542 | ENDM 543 | Clear_LED_1 MACRO 544 | ENDM 545 | Set_LED_2 MACRO 546 | ENDM 547 | Clear_LED_2 MACRO 548 | ENDM 549 | Set_LED_3 MACRO 550 | ENDM 551 | Clear_LED_3 MACRO 552 | ENDM 553 | -------------------------------------------------------------------------------- /src/L.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "L" 26 | ; X X RC X CC MA MB MC X X Ac Bc Cc Ap Bp Cp 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#L_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#L_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#L_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#L_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#L_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#L_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#L_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#L_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#L_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#L_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#L_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#L_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#L_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#L_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#L_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#L_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#L_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#L_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#L_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#L_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#L_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#L_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Comp_Com EQU 3 ;i 134 | Mux_A EQU 2 ;i 135 | Mux_B EQU 1 ;i 136 | Mux_C EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | ;********************* 316 | ; PORT 1 definitions * 317 | ;********************* 318 | ; EQU 7 ;i 319 | ; EQU 6 ;i 320 | AcomFET EQU 5 ;o 321 | BcomFET EQU 4 ;o 322 | CcomFET EQU 3 ;o 323 | ApwmFET EQU 2 ;o 324 | BpwmFET EQU 1 ;o 325 | CpwmFET EQU 0 ;o 326 | 327 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 328 | P1_INIT EQU 00h 329 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 330 | P1_SKIP EQU 3Fh 331 | 332 | ApwmFET_on MACRO 333 | setb P1.ApwmFET 334 | IF FETON_DELAY == 0 335 | setb P1.AcomFET 336 | ENDIF 337 | ENDM 338 | ApwmFET_off MACRO 339 | IF FETON_DELAY != 0 340 | clr P1.ApwmFET 341 | ELSE 342 | clr P1.AcomFET 343 | ENDIF 344 | ENDM 345 | BpwmFET_on MACRO 346 | setb P1.BpwmFET 347 | IF FETON_DELAY == 0 348 | setb P1.BcomFET 349 | ENDIF 350 | ENDM 351 | BpwmFET_off MACRO 352 | IF FETON_DELAY != 0 353 | clr P1.BpwmFET 354 | ELSE 355 | clr P1.BcomFET 356 | ENDIF 357 | ENDM 358 | CpwmFET_on MACRO 359 | setb P1.CpwmFET 360 | IF FETON_DELAY == 0 361 | setb P1.CcomFET 362 | ENDIF 363 | ENDM 364 | CpwmFET_off MACRO 365 | IF FETON_DELAY != 0 366 | clr P1.CpwmFET 367 | ELSE 368 | clr P1.CcomFET 369 | ENDIF 370 | ENDM 371 | All_pwmFETs_Off MACRO 372 | IF FETON_DELAY != 0 373 | clr P1.ApwmFET 374 | clr P1.BpwmFET 375 | clr P1.CpwmFET 376 | ELSE 377 | clr P1.AcomFET 378 | clr P1.BcomFET 379 | clr P1.CcomFET 380 | ENDIF 381 | ENDM 382 | 383 | AcomFET_on MACRO 384 | IF FETON_DELAY == 0 385 | clr P1.ApwmFET 386 | ENDIF 387 | setb P1.AcomFET 388 | ENDM 389 | AcomFET_off MACRO 390 | clr P1.AcomFET 391 | ENDM 392 | BcomFET_on MACRO 393 | IF FETON_DELAY == 0 394 | clr P1.BpwmFET 395 | ENDIF 396 | setb P1.BcomFET 397 | ENDM 398 | BcomFET_off MACRO 399 | clr P1.BcomFET 400 | ENDM 401 | CcomFET_on MACRO 402 | IF FETON_DELAY == 0 403 | clr P1.CpwmFET 404 | ENDIF 405 | setb P1.CcomFET 406 | ENDM 407 | CcomFET_off MACRO 408 | clr P1.CcomFET 409 | ENDM 410 | All_comFETs_Off MACRO 411 | clr P1.AcomFET 412 | clr P1.BcomFET 413 | clr P1.CcomFET 414 | ENDM 415 | 416 | Set_Pwm_A MACRO 417 | IF FETON_DELAY == 0 418 | setb P1.AcomFET 419 | mov P1SKIP, #3Bh 420 | ELSE 421 | mov P1SKIP, #1Bh 422 | ENDIF 423 | ENDM 424 | Set_Pwm_B MACRO 425 | IF FETON_DELAY == 0 426 | setb P1.BcomFET 427 | mov P1SKIP, #3Dh 428 | ELSE 429 | mov P1SKIP, #2Dh 430 | ENDIF 431 | ENDM 432 | Set_Pwm_C MACRO 433 | IF FETON_DELAY == 0 434 | setb P1.CcomFET 435 | mov P1SKIP, #3Eh 436 | ELSE 437 | mov P1SKIP, #36h 438 | ENDIF 439 | ENDM 440 | Set_Pwms_Off MACRO 441 | mov P1SKIP, #7Fh 442 | ENDM 443 | 444 | Set_Comp_Phase_A MACRO 445 | mov CMP0MX, #23h ; Set comparator multiplexer to phase A 446 | ENDM 447 | Set_Comp_Phase_B MACRO 448 | mov CMP0MX, #13h ; Set comparator multiplexer to phase B 449 | ENDM 450 | Set_Comp_Phase_C MACRO 451 | mov CMP0MX, #03h ; Set comparator multiplexer to phase C 452 | ENDM 453 | Read_Comp_Out MACRO 454 | mov A, CMP0CN0 ; Read comparator output 455 | ENDM 456 | 457 | 458 | ;********************* 459 | ; PORT 2 definitions * 460 | ;********************* 461 | DebugPin EQU 0 ;o 462 | 463 | P2_PUSHPULL EQU (1 SHL DebugPin) 464 | 465 | 466 | ;********************** 467 | ; MCU specific macros * 468 | ;********************** 469 | Interrupt_Table_Definition MACRO 470 | CSEG AT 0 ; Code segment start 471 | jmp reset 472 | CSEG AT 03h ; Int0 interrupt 473 | jmp int0_int 474 | IF MCU_48MHZ == 1 475 | CSEG AT 0Bh ; Timer0 overflow interrupt 476 | jmp t0_int 477 | ENDIF 478 | CSEG AT 13h ; Int1 interrupt 479 | jmp int1_int 480 | CSEG AT 1Bh ; Timer1 overflow interrupt 481 | jmp t1_int 482 | CSEG AT 2Bh ; Timer2 overflow interrupt 483 | jmp t2_int 484 | CSEG AT 5Bh ; Pca interrupt 485 | jmp pca_int 486 | CSEG AT 73h ; Timer3 overflow/compare interrupt 487 | jmp t3_int 488 | ENDM 489 | 490 | Initialize_Xbar MACRO 491 | mov XBR2, #40h ; Xbar enabled 492 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 493 | ENDM 494 | 495 | Initialize_Comparator MACRO 496 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 497 | mov CMP0MD, #00h ; Comparator response time 100ns 498 | ENDM 499 | Initialize_Adc MACRO 500 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 501 | IF MCU_48MHZ == 0 502 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 503 | ELSE 504 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 505 | ENDIF 506 | mov ADC0MX, #10h ; Select temp sensor input 507 | mov ADC0CN0, #80h ; ADC enabled 508 | mov ADC0CN1, #01h ; Common mode buffer enabled 509 | ENDM 510 | Start_Adc MACRO 511 | mov ADC0CN0, #90h ; ADC start 512 | ENDM 513 | Read_Adc_Result MACRO 514 | mov Temp1, ADC0L 515 | mov Temp2, ADC0H 516 | ENDM 517 | Stop_Adc MACRO 518 | ENDM 519 | Set_RPM_Out MACRO 520 | ENDM 521 | Clear_RPM_Out MACRO 522 | ENDM 523 | Set_MCU_Clk_24MHz MACRO 524 | mov CLKSEL, #13h ; Set clock to 24MHz 525 | mov SFRPAGE, #10h 526 | mov PFE0CN, #00h ; Set flash timing for 24MHz 527 | mov SFRPAGE, #00h 528 | mov Clock_Set_At_48MHz, #0 529 | ENDM 530 | Set_MCU_Clk_48MHz MACRO 531 | mov SFRPAGE, #10h 532 | mov PFE0CN, #30h ; Set flash timing for 48MHz 533 | mov SFRPAGE, #00h 534 | mov CLKSEL, #03h ; Set clock to 48MHz 535 | mov Clock_Set_At_48MHz, #1 536 | ENDM 537 | Set_LED_0 MACRO 538 | ENDM 539 | Clear_LED_0 MACRO 540 | ENDM 541 | Set_LED_1 MACRO 542 | ENDM 543 | Clear_LED_1 MACRO 544 | ENDM 545 | Set_LED_2 MACRO 546 | ENDM 547 | Clear_LED_2 MACRO 548 | ENDM 549 | Set_LED_3 MACRO 550 | ENDM 551 | Clear_LED_3 MACRO 552 | ENDM 553 | -------------------------------------------------------------------------------- /src/M.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "M". 26 | ; MA MC CC MB RC L0 X X X Cc Bc Ac Cp Bp Ap X 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#M_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#M_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#M_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#M_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#M_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#M_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#M_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#M_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#M_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#M_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#M_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#M_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#M_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#M_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#M_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#M_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#M_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#M_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#M_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#M_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#M_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#M_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 3 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | 127 | ;********************* 128 | ; PORT 0 definitions * 129 | ;********************* 130 | Mux_A EQU 7 ;i 131 | Mux_C EQU 6 ;i 132 | Comp_Com EQU 5 ;i 133 | Mux_B EQU 4 ;i 134 | Rcp_In EQU 3 ;i 135 | LED_0 EQU 2 ;i 136 | ; EQU 1 ;i 137 | ; EQU 0 ;i 138 | 139 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 140 | P0_INIT EQU NOT(1 SHL LED_0) 141 | P0_PUSHPULL EQU (1 SHL LED_0) 142 | P0_SKIP EQU 0FFh 143 | 144 | Get_Rcp_Capture_Values MACRO 145 | anl TCON, #0EFh ; Disable timer0 146 | mov Temp1, TL0 ; Get timer0 values 147 | mov Temp2, TH0 148 | IF MCU_48MHZ == 1 149 | mov Temp3, Timer0_X 150 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 151 | inc Temp3 ; If it is pending, then timer has already wrapped 152 | ENDIF 153 | mov TL0, #0 ; Reset timer0 154 | mov TH0, #0 155 | IF MCU_48MHZ == 1 156 | mov Timer0_X, #0 157 | ENDIF 158 | orl TCON, #10h ; Enable timer0 again 159 | IF MCU_48MHZ == 1 160 | mov A, Clock_Set_At_48MHz 161 | jnz Get_Rcp_End 162 | clr C 163 | mov A, Temp1 164 | rlc A 165 | mov Temp1, A 166 | mov A, Temp2 167 | rlc A 168 | mov Temp2, A 169 | mov A, Temp3 170 | rlc A 171 | mov Temp3, A 172 | Get_Rcp_End: 173 | ENDIF 174 | ENDM 175 | Decode_Dshot_2Msb MACRO 176 | movx A, @DPTR 177 | mov Temp6, A 178 | clr C 179 | subb A, Temp5 ; Subtract previous timestamp 180 | clr C 181 | subb A, Temp1 182 | jc t1_int_msb_fail ; Check that bit is longer than minimum 183 | 184 | subb A, Temp1 ; Check if bit is zero or one 185 | mov A, Temp4 ; Shift bit into data byte 186 | rlc A 187 | mov Temp4, A 188 | inc DPL ; Next bit 189 | movx A, @DPTR 190 | mov Temp5, A 191 | clr C 192 | subb A, Temp6 193 | clr C 194 | subb A, Temp1 195 | jc t1_int_msb_fail 196 | 197 | subb A, Temp1 198 | mov A, Temp4 199 | rlc A 200 | mov Temp4, A 201 | inc DPL 202 | ENDM 203 | Decode_Dshot_2Lsb MACRO 204 | movx A, @DPTR 205 | mov Temp6, A 206 | clr C 207 | subb A, Temp5 ; Subtract previous timestamp 208 | clr C 209 | subb A, Temp1 210 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 211 | 212 | subb A, Temp1 ; Check if bit is zero or one 213 | mov A, Temp3 ; Shift bit into data byte 214 | rlc A 215 | mov Temp3, A 216 | inc DPL ; Next bit 217 | movx A, @DPTR 218 | mov Temp5, A 219 | clr C 220 | subb A, Temp6 221 | clr C 222 | subb A, Temp1 223 | jc t1_int_lsb_fail 224 | 225 | subb A, Temp1 226 | mov A, Temp3 227 | rlc A 228 | mov Temp3, A 229 | inc DPL 230 | ENDM 231 | Initialize_PCA MACRO 232 | mov PCA0CN0, #40h ; PCA enabled 233 | mov PCA0MD, #08h ; PCA clock is system clock 234 | IF FETON_DELAY == 0 235 | IF MCU_48MHZ == 0 236 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 237 | ELSE 238 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 239 | ENDIF 240 | mov PCA0CENT, #00h ; Edge aligned pwm 241 | ELSE 242 | IF MCU_48MHZ == 0 243 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 244 | ELSE 245 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 246 | ENDIF 247 | mov PCA0CENT, #03h ; Center aligned pwm 248 | ENDIF 249 | ENDM 250 | Set_Pwm_Polarity MACRO 251 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 252 | ENDM 253 | Enable_Power_Pwm_Module MACRO 254 | IF FETON_DELAY == 0 255 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 256 | ELSE 257 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 258 | ENDIF 259 | ENDM 260 | Enable_Damp_Pwm_Module MACRO 261 | IF FETON_DELAY == 0 262 | mov PCA0CPM1, #00h ; Disable 263 | ELSE 264 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 265 | ENDIF 266 | ENDM 267 | Set_Power_Pwm_Regs MACRO 268 | IF FETON_DELAY == 0 269 | mov PCA0CPL0, Power_Pwm_Reg_L 270 | mov PCA0CPH0, Power_Pwm_Reg_H 271 | ELSE 272 | clr C 273 | mov A, Power_Pwm_Reg_H 274 | rrc A 275 | mov Temp1, A 276 | mov A, Power_Pwm_Reg_L 277 | rrc A 278 | mov PCA0CPL0, A 279 | mov PCA0CPH0, Temp1 280 | ENDIF 281 | ENDM 282 | Set_Damp_Pwm_Regs MACRO 283 | IF FETON_DELAY == 0 284 | mov PCA0CPL1, Damp_Pwm_Reg_L 285 | mov PCA0CPH1, Damp_Pwm_Reg_H 286 | ELSE 287 | clr C 288 | mov A, Damp_Pwm_Reg_H 289 | rrc A 290 | mov Temp1, A 291 | mov A, Damp_Pwm_Reg_L 292 | rrc A 293 | mov PCA0CPL1, A 294 | mov PCA0CPH1, Temp1 295 | ENDIF 296 | ENDM 297 | Clear_COVF_Interrupt MACRO 298 | anl PCA0PWM, #0DFh 299 | ENDM 300 | Clear_CCF_Interrupt MACRO 301 | anl PCA0CN0, #0FEh 302 | ENDM 303 | Enable_COVF_Interrupt MACRO 304 | orl PCA0PWM, #40h 305 | ENDM 306 | Enable_CCF_Interrupt MACRO 307 | orl PCA0CPM0,#01h 308 | ENDM 309 | Disable_COVF_Interrupt MACRO 310 | anl PCA0PWM, #0BFh 311 | ENDM 312 | Disable_CCF_Interrupt MACRO 313 | anl PCA0CPM0,#0FEh 314 | ENDM 315 | 316 | 317 | 318 | ;********************* 319 | ; PORT 1 definitions * 320 | ;********************* 321 | ; EQU 7 ;i 322 | CcomFET EQU 6 ;o 323 | BcomFET EQU 5 ;o 324 | AcomFET EQU 4 ;o 325 | CpwmFET EQU 3 ;i 326 | BpwmFET EQU 2 ;o 327 | ApwmFET EQU 1 ;o 328 | ; EQU 0 ;o 329 | 330 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_INIT EQU 00h 332 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 333 | P1_SKIP EQU 7Fh 334 | 335 | ApwmFET_on MACRO 336 | setb P1.ApwmFET 337 | IF FETON_DELAY == 0 338 | setb P1.AcomFET 339 | ENDIF 340 | ENDM 341 | ApwmFET_off MACRO 342 | IF FETON_DELAY != 0 343 | clr P1.ApwmFET 344 | ELSE 345 | clr P1.AcomFET 346 | ENDIF 347 | ENDM 348 | BpwmFET_on MACRO 349 | setb P1.BpwmFET 350 | IF FETON_DELAY == 0 351 | setb P1.BcomFET 352 | ENDIF 353 | ENDM 354 | BpwmFET_off MACRO 355 | IF FETON_DELAY != 0 356 | clr P1.BpwmFET 357 | ELSE 358 | clr P1.BcomFET 359 | ENDIF 360 | ENDM 361 | CpwmFET_on MACRO 362 | setb P1.CpwmFET 363 | IF FETON_DELAY == 0 364 | setb P1.CcomFET 365 | ENDIF 366 | ENDM 367 | CpwmFET_off MACRO 368 | IF FETON_DELAY != 0 369 | clr P1.CpwmFET 370 | ELSE 371 | clr P1.CcomFET 372 | ENDIF 373 | ENDM 374 | All_pwmFETs_Off MACRO 375 | IF FETON_DELAY != 0 376 | clr P1.ApwmFET 377 | clr P1.BpwmFET 378 | clr P1.CpwmFET 379 | ELSE 380 | clr P1.AcomFET 381 | clr P1.BcomFET 382 | clr P1.CcomFET 383 | ENDIF 384 | ENDM 385 | 386 | AcomFET_on MACRO 387 | IF FETON_DELAY == 0 388 | clr P1.ApwmFET 389 | ENDIF 390 | setb P1.AcomFET 391 | ENDM 392 | AcomFET_off MACRO 393 | clr P1.AcomFET 394 | ENDM 395 | BcomFET_on MACRO 396 | IF FETON_DELAY == 0 397 | clr P1.BpwmFET 398 | ENDIF 399 | setb P1.BcomFET 400 | ENDM 401 | BcomFET_off MACRO 402 | clr P1.BcomFET 403 | ENDM 404 | CcomFET_on MACRO 405 | IF FETON_DELAY == 0 406 | clr P1.CpwmFET 407 | ENDIF 408 | setb P1.CcomFET 409 | ENDM 410 | CcomFET_off MACRO 411 | clr P1.CcomFET 412 | ENDM 413 | All_comFETs_Off MACRO 414 | clr P1.AcomFET 415 | clr P1.BcomFET 416 | clr P1.CcomFET 417 | ENDM 418 | 419 | Set_Pwm_A MACRO 420 | IF FETON_DELAY == 0 421 | setb P1.AcomFET 422 | mov P1SKIP, #7Dh 423 | ELSE 424 | mov P1SKIP, #6Dh 425 | ENDIF 426 | ENDM 427 | Set_Pwm_B MACRO 428 | IF FETON_DELAY == 0 429 | setb P1.BcomFET 430 | mov P1SKIP, #7Bh 431 | ELSE 432 | mov P1SKIP, #5Bh 433 | ENDIF 434 | ENDM 435 | Set_Pwm_C MACRO 436 | IF FETON_DELAY == 0 437 | setb P1.CcomFET 438 | mov P1SKIP, #77h 439 | ELSE 440 | mov P1SKIP, #37h 441 | ENDIF 442 | ENDM 443 | Set_Pwms_Off MACRO 444 | mov P1SKIP, #7Fh 445 | ENDM 446 | 447 | Set_Comp_Phase_A MACRO 448 | mov CMP0MX, #75h ; Set comparator multiplexer to phase A 449 | ENDM 450 | Set_Comp_Phase_B MACRO 451 | mov CMP0MX, #45h ; Set comparator multiplexer to phase B 452 | ENDM 453 | Set_Comp_Phase_C MACRO 454 | mov CMP0MX, #65h ; Set comparator multiplexer to phase C 455 | ENDM 456 | Read_Comp_Out MACRO 457 | mov A, CMP0CN0 ; Read comparator output 458 | ENDM 459 | 460 | 461 | ;********************* 462 | ; PORT 2 definitions * 463 | ;********************* 464 | DebugPin EQU 0 ;o 465 | 466 | P2_PUSHPULL EQU (1 SHL DebugPin) 467 | 468 | 469 | ;********************** 470 | ; MCU specific macros * 471 | ;********************** 472 | Interrupt_Table_Definition MACRO 473 | CSEG AT 0 ; Code segment start 474 | jmp reset 475 | CSEG AT 03h ; Int0 interrupt 476 | jmp int0_int 477 | IF MCU_48MHZ == 1 478 | CSEG AT 0Bh ; Timer0 overflow interrupt 479 | jmp t0_int 480 | ENDIF 481 | CSEG AT 13h ; Int1 interrupt 482 | jmp int1_int 483 | CSEG AT 1Bh ; Timer1 overflow interrupt 484 | jmp t1_int 485 | CSEG AT 2Bh ; Timer2 overflow interrupt 486 | jmp t2_int 487 | CSEG AT 5Bh ; Pca interrupt 488 | jmp pca_int 489 | CSEG AT 73h ; Timer3 overflow/compare interrupt 490 | jmp t3_int 491 | ENDM 492 | 493 | Initialize_Xbar MACRO 494 | mov XBR2, #40h ; Xbar enabled 495 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 496 | ENDM 497 | 498 | Initialize_Comparator MACRO 499 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 500 | mov CMP0MD, #00h ; Comparator response time 100ns 501 | ENDM 502 | Initialize_Adc MACRO 503 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 504 | IF MCU_48MHZ == 0 505 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 506 | ELSE 507 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 508 | ENDIF 509 | mov ADC0MX, #10h ; Select temp sensor input 510 | mov ADC0CN0, #80h ; ADC enabled 511 | mov ADC0CN1, #01h ; Common mode buffer enabled 512 | ENDM 513 | Start_Adc MACRO 514 | mov ADC0CN0, #90h ; ADC start 515 | ENDM 516 | Read_Adc_Result MACRO 517 | mov Temp1, ADC0L 518 | mov Temp2, ADC0H 519 | ENDM 520 | Stop_Adc MACRO 521 | ENDM 522 | Set_RPM_Out MACRO 523 | ENDM 524 | Clear_RPM_Out MACRO 525 | ENDM 526 | Set_MCU_Clk_24MHz MACRO 527 | mov CLKSEL, #13h ; Set clock to 24MHz 528 | mov SFRPAGE, #10h 529 | mov PFE0CN, #00h ; Set flash timing for 24MHz 530 | mov SFRPAGE, #00h 531 | mov Clock_Set_At_48MHz, #0 532 | ENDM 533 | Set_MCU_Clk_48MHz MACRO 534 | mov SFRPAGE, #10h 535 | mov PFE0CN, #30h ; Set flash timing for 48MHz 536 | mov SFRPAGE, #00h 537 | mov CLKSEL, #03h ; Set clock to 48MHz 538 | mov Clock_Set_At_48MHz, #1 539 | ENDM 540 | Set_LED_0 MACRO 541 | setb P0.LED_0 542 | ENDM 543 | Clear_LED_0 MACRO 544 | clr P0.LED_0 545 | ENDM 546 | Set_LED_1 MACRO 547 | ENDM 548 | Clear_LED_1 MACRO 549 | ENDM 550 | Set_LED_2 MACRO 551 | ENDM 552 | Clear_LED_2 MACRO 553 | ENDM 554 | Set_LED_3 MACRO 555 | ENDM 556 | Clear_LED_3 MACRO 557 | ENDM 558 | -------------------------------------------------------------------------------- /src/N.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "N" 26 | ; X X RC X MC MB MA CC X X Cp Cc Bp Bc Ap Ac 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#N_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#N_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#N_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#N_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#N_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#N_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#N_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#N_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#N_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#N_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#N_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#N_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#N_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#N_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#N_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#N_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#N_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#N_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#N_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#N_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#N_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#N_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | 127 | ;********************* 128 | ; PORT 0 definitions * 129 | ;********************* 130 | ; EQU 7 ;i 131 | ; EQU 6 ;i 132 | Rcp_In EQU 5 ;i 133 | ; EQU 4 ;i 134 | Mux_C EQU 3 ;i 135 | Mux_B EQU 2 ;i 136 | Mux_A EQU 1 ;i 137 | Comp_Com EQU 0 ;i 138 | 139 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 140 | P0_INIT EQU 0FFh 141 | P0_PUSHPULL EQU 0 142 | P0_SKIP EQU 0FFh 143 | 144 | Get_Rcp_Capture_Values MACRO 145 | anl TCON, #0EFh ; Disable timer0 146 | mov Temp1, TL0 ; Get timer0 values 147 | mov Temp2, TH0 148 | IF MCU_48MHZ == 1 149 | mov Temp3, Timer0_X 150 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 151 | inc Temp3 ; If it is pending, then timer has already wrapped 152 | ENDIF 153 | mov TL0, #0 ; Reset timer0 154 | mov TH0, #0 155 | IF MCU_48MHZ == 1 156 | mov Timer0_X, #0 157 | ENDIF 158 | orl TCON, #10h ; Enable timer0 again 159 | IF MCU_48MHZ == 1 160 | mov A, Clock_Set_At_48MHz 161 | jnz Get_Rcp_End 162 | clr C 163 | mov A, Temp1 164 | rlc A 165 | mov Temp1, A 166 | mov A, Temp2 167 | rlc A 168 | mov Temp2, A 169 | mov A, Temp3 170 | rlc A 171 | mov Temp3, A 172 | Get_Rcp_End: 173 | ENDIF 174 | ENDM 175 | Decode_Dshot_2Msb MACRO 176 | movx A, @DPTR 177 | mov Temp6, A 178 | clr C 179 | subb A, Temp5 ; Subtract previous timestamp 180 | clr C 181 | subb A, Temp1 182 | jc t1_int_msb_fail ; Check that bit is longer than minimum 183 | 184 | subb A, Temp1 ; Check if bit is zero or one 185 | mov A, Temp4 ; Shift bit into data byte 186 | rlc A 187 | mov Temp4, A 188 | inc DPL ; Next bit 189 | movx A, @DPTR 190 | mov Temp5, A 191 | clr C 192 | subb A, Temp6 193 | clr C 194 | subb A, Temp1 195 | jc t1_int_msb_fail 196 | 197 | subb A, Temp1 198 | mov A, Temp4 199 | rlc A 200 | mov Temp4, A 201 | inc DPL 202 | ENDM 203 | Decode_Dshot_2Lsb MACRO 204 | movx A, @DPTR 205 | mov Temp6, A 206 | clr C 207 | subb A, Temp5 ; Subtract previous timestamp 208 | clr C 209 | subb A, Temp1 210 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 211 | 212 | subb A, Temp1 ; Check if bit is zero or one 213 | mov A, Temp3 ; Shift bit into data byte 214 | rlc A 215 | mov Temp3, A 216 | inc DPL ; Next bit 217 | movx A, @DPTR 218 | mov Temp5, A 219 | clr C 220 | subb A, Temp6 221 | clr C 222 | subb A, Temp1 223 | jc t1_int_lsb_fail 224 | 225 | subb A, Temp1 226 | mov A, Temp3 227 | rlc A 228 | mov Temp3, A 229 | inc DPL 230 | ENDM 231 | Initialize_PCA MACRO 232 | mov PCA0CN0, #40h ; PCA enabled 233 | mov PCA0MD, #08h ; PCA clock is system clock 234 | IF FETON_DELAY == 0 235 | IF MCU_48MHZ == 0 236 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 237 | ELSE 238 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 239 | ENDIF 240 | mov PCA0CENT, #00h ; Edge aligned pwm 241 | ELSE 242 | IF MCU_48MHZ == 0 243 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 244 | ELSE 245 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 246 | ENDIF 247 | mov PCA0CENT, #03h ; Center aligned pwm 248 | ENDIF 249 | ENDM 250 | Set_Pwm_Polarity MACRO 251 | IF FETON_DELAY == 0 252 | mov PCA0POL, #00h ; Pwm noninverted 253 | ELSE 254 | mov PCA0POL, #01h ; Damping inverted, pwm noninverted 255 | ENDIF 256 | ENDM 257 | Enable_Power_Pwm_Module MACRO 258 | IF FETON_DELAY == 0 259 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 260 | ELSE 261 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 262 | ENDIF 263 | ENDM 264 | Enable_Damp_Pwm_Module MACRO 265 | IF FETON_DELAY == 0 266 | mov PCA0CPM1, #00h ; Disable 267 | ELSE 268 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 269 | ENDIF 270 | ENDM 271 | Set_Power_Pwm_Regs MACRO 272 | IF FETON_DELAY == 0 273 | mov PCA0CPL0, Power_Pwm_Reg_L 274 | mov PCA0CPH0, Power_Pwm_Reg_H 275 | ELSE 276 | clr C 277 | mov A, Power_Pwm_Reg_H 278 | rrc A 279 | mov Temp1, A 280 | mov A, Power_Pwm_Reg_L 281 | rrc A 282 | mov PCA0CPL1, A 283 | mov PCA0CPH1, Temp1 284 | ENDIF 285 | ENDM 286 | Set_Damp_Pwm_Regs MACRO 287 | IF FETON_DELAY == 0 288 | mov PCA0CPL1, Damp_Pwm_Reg_L 289 | mov PCA0CPH1, Damp_Pwm_Reg_H 290 | ELSE 291 | clr C 292 | mov A, Damp_Pwm_Reg_H 293 | rrc A 294 | mov Temp1, A 295 | mov A, Damp_Pwm_Reg_L 296 | rrc A 297 | mov PCA0CPL0, A 298 | mov PCA0CPH0, Temp1 299 | ENDIF 300 | ENDM 301 | Clear_COVF_Interrupt MACRO 302 | anl PCA0PWM, #0DFh 303 | ENDM 304 | Clear_CCF_Interrupt MACRO ; CCF interrupt is only used for FETON_DELAY == 0 305 | anl PCA0CN0, #0FEh 306 | ENDM 307 | Enable_COVF_Interrupt MACRO 308 | orl PCA0PWM, #40h 309 | ENDM 310 | Enable_CCF_Interrupt MACRO 311 | orl PCA0CPM0,#01h 312 | ENDM 313 | Disable_COVF_Interrupt MACRO 314 | anl PCA0PWM, #0BFh 315 | ENDM 316 | Disable_CCF_Interrupt MACRO 317 | anl PCA0CPM0,#0FEh 318 | ENDM 319 | 320 | 321 | ;********************* 322 | ; PORT 1 definitions * 323 | ;********************* 324 | ; EQU 7 ;i 325 | ; EQU 6 ;i 326 | CpwmFET EQU 5 ;o 327 | CcomFET EQU 4 ;o 328 | BpwmFET EQU 3 ;o 329 | BcomFET EQU 2 ;o 330 | ApwmFET EQU 1 ;o 331 | AcomFET EQU 0 ;o 332 | 333 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 334 | P1_INIT EQU 00h 335 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 336 | P1_SKIP EQU 3Fh 337 | 338 | ApwmFET_on MACRO 339 | setb P1.ApwmFET 340 | IF FETON_DELAY == 0 341 | setb P1.AcomFET 342 | ENDIF 343 | ENDM 344 | ApwmFET_off MACRO 345 | IF FETON_DELAY != 0 346 | clr P1.ApwmFET 347 | ELSE 348 | clr P1.AcomFET 349 | ENDIF 350 | ENDM 351 | BpwmFET_on MACRO 352 | setb P1.BpwmFET 353 | IF FETON_DELAY == 0 354 | setb P1.BcomFET 355 | ENDIF 356 | ENDM 357 | BpwmFET_off MACRO 358 | IF FETON_DELAY != 0 359 | clr P1.BpwmFET 360 | ELSE 361 | clr P1.BcomFET 362 | ENDIF 363 | ENDM 364 | CpwmFET_on MACRO 365 | setb P1.CpwmFET 366 | IF FETON_DELAY == 0 367 | setb P1.CcomFET 368 | ENDIF 369 | ENDM 370 | CpwmFET_off MACRO 371 | IF FETON_DELAY != 0 372 | clr P1.CpwmFET 373 | ELSE 374 | clr P1.CcomFET 375 | ENDIF 376 | ENDM 377 | All_pwmFETs_Off MACRO 378 | IF FETON_DELAY != 0 379 | clr P1.ApwmFET 380 | clr P1.BpwmFET 381 | clr P1.CpwmFET 382 | ELSE 383 | clr P1.AcomFET 384 | clr P1.BcomFET 385 | clr P1.CcomFET 386 | ENDIF 387 | ENDM 388 | 389 | AcomFET_on MACRO 390 | IF FETON_DELAY == 0 391 | clr P1.ApwmFET 392 | ENDIF 393 | setb P1.AcomFET 394 | ENDM 395 | AcomFET_off MACRO 396 | clr P1.AcomFET 397 | ENDM 398 | BcomFET_on MACRO 399 | IF FETON_DELAY == 0 400 | clr P1.BpwmFET 401 | ENDIF 402 | setb P1.BcomFET 403 | ENDM 404 | BcomFET_off MACRO 405 | clr P1.BcomFET 406 | ENDM 407 | CcomFET_on MACRO 408 | IF FETON_DELAY == 0 409 | clr P1.CpwmFET 410 | ENDIF 411 | setb P1.CcomFET 412 | ENDM 413 | CcomFET_off MACRO 414 | clr P1.CcomFET 415 | ENDM 416 | All_comFETs_Off MACRO 417 | clr P1.AcomFET 418 | clr P1.BcomFET 419 | clr P1.CcomFET 420 | ENDM 421 | 422 | Set_Pwm_A MACRO 423 | IF FETON_DELAY == 0 424 | setb P1.AcomFET 425 | mov P1SKIP, #3Dh 426 | ELSE 427 | mov P1SKIP, #3Ch 428 | ENDIF 429 | ENDM 430 | Set_Pwm_B MACRO 431 | IF FETON_DELAY == 0 432 | setb P1.BcomFET 433 | mov P1SKIP, #37h 434 | ELSE 435 | mov P1SKIP, #33h 436 | ENDIF 437 | ENDM 438 | Set_Pwm_C MACRO 439 | IF FETON_DELAY == 0 440 | setb P1.CcomFET 441 | mov P1SKIP, #1Fh 442 | ELSE 443 | mov P1SKIP, #0Fh 444 | ENDIF 445 | ENDM 446 | Set_Pwms_Off MACRO 447 | mov P1SKIP, #3Fh 448 | ENDM 449 | 450 | Set_Comp_Phase_A MACRO 451 | mov CMP0MX, #10h ; Set comparator multiplexer to phase A 452 | ENDM 453 | Set_Comp_Phase_B MACRO 454 | mov CMP0MX, #20h ; Set comparator multiplexer to phase B 455 | ENDM 456 | Set_Comp_Phase_C MACRO 457 | mov CMP0MX, #30h ; Set comparator multiplexer to phase C 458 | ENDM 459 | Read_Comp_Out MACRO 460 | mov A, CMP0CN0 ; Read comparator output 461 | ENDM 462 | 463 | 464 | ;********************* 465 | ; PORT 2 definitions * 466 | ;********************* 467 | DebugPin EQU 0 ;o 468 | 469 | P2_PUSHPULL EQU (1 SHL DebugPin) 470 | 471 | 472 | ;********************** 473 | ; MCU specific macros * 474 | ;********************** 475 | Interrupt_Table_Definition MACRO 476 | CSEG AT 0 ; Code segment start 477 | jmp reset 478 | CSEG AT 03h ; Int0 interrupt 479 | jmp int0_int 480 | IF MCU_48MHZ == 1 481 | CSEG AT 0Bh ; Timer0 overflow interrupt 482 | jmp t0_int 483 | ENDIF 484 | CSEG AT 13h ; Int1 interrupt 485 | jmp int1_int 486 | CSEG AT 1Bh ; Timer1 overflow interrupt 487 | jmp t1_int 488 | CSEG AT 2Bh ; Timer2 overflow interrupt 489 | jmp t2_int 490 | CSEG AT 5Bh ; Pca interrupt 491 | jmp pca_int 492 | CSEG AT 73h ; Timer3 overflow/compare interrupt 493 | jmp t3_int 494 | ENDM 495 | 496 | Initialize_Xbar MACRO 497 | mov XBR2, #40h ; Xbar enabled 498 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 499 | ENDM 500 | 501 | Initialize_Comparator MACRO 502 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 503 | mov CMP0MD, #00h ; Comparator response time 100ns 504 | ENDM 505 | Initialize_Adc MACRO 506 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 507 | IF MCU_48MHZ == 0 508 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 509 | ELSE 510 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 511 | ENDIF 512 | mov ADC0MX, #10h ; Select temp sensor input 513 | mov ADC0CN0, #80h ; ADC enabled 514 | mov ADC0CN1, #01h ; Common mode buffer enabled 515 | ENDM 516 | Start_Adc MACRO 517 | mov ADC0CN0, #90h ; ADC start 518 | ENDM 519 | Read_Adc_Result MACRO 520 | mov Temp1, ADC0L 521 | mov Temp2, ADC0H 522 | ENDM 523 | Stop_Adc MACRO 524 | ENDM 525 | Set_RPM_Out MACRO 526 | ENDM 527 | Clear_RPM_Out MACRO 528 | ENDM 529 | Set_MCU_Clk_24MHz MACRO 530 | mov CLKSEL, #13h ; Set clock to 24MHz 531 | mov SFRPAGE, #10h 532 | mov PFE0CN, #00h ; Set flash timing for 24MHz 533 | mov SFRPAGE, #00h 534 | mov Clock_Set_At_48MHz, #0 535 | ENDM 536 | Set_MCU_Clk_48MHz MACRO 537 | mov SFRPAGE, #10h 538 | mov PFE0CN, #30h ; Set flash timing for 48MHz 539 | mov SFRPAGE, #00h 540 | mov CLKSEL, #03h ; Set clock to 48MHz 541 | mov Clock_Set_At_48MHz, #1 542 | ENDM 543 | Set_LED_0 MACRO 544 | ENDM 545 | Clear_LED_0 MACRO 546 | ENDM 547 | Set_LED_1 MACRO 548 | ENDM 549 | Clear_LED_1 MACRO 550 | ENDM 551 | Set_LED_2 MACRO 552 | ENDM 553 | Clear_LED_2 MACRO 554 | ENDM 555 | Set_LED_3 MACRO 556 | ENDM 557 | Clear_LED_3 MACRO 558 | ENDM 559 | -------------------------------------------------------------------------------- /src/O.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "O". Com fets are active low for H/L_N driver and EN_N/PWM driver. Low side pwm and 1S flag set 26 | ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#O_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#O_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#O_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#O_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#O_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#O_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#O_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#O_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#O_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#O_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#O_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#O_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#O_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#O_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#O_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#O_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#O_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#O_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#O_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#O_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#O_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#O_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Comp_Com EQU 3 ;i 134 | Mux_A EQU 2 ;i 135 | Mux_C EQU 1 ;i 136 | Mux_B EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #00h ; Damping noninverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | 316 | ;********************* 317 | ; PORT 1 definitions * 318 | ;********************* 319 | ; EQU 7 ;i 320 | ; EQU 6 ;i 321 | CcomFET EQU 5 ;o 322 | CpwmFET EQU 4 ;o 323 | BcomFET EQU 3 ;o 324 | BpwmFET EQU 2 ;o 325 | AcomFET EQU 1 ;o 326 | ApwmFET EQU 0 ;o 327 | 328 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 329 | P1_INIT EQU (1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 330 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_SKIP EQU 3Fh 332 | 333 | ApwmFET_on MACRO 334 | setb P1.ApwmFET 335 | IF FETON_DELAY == 0 336 | clr P1.AcomFET 337 | ENDIF 338 | ENDM 339 | ApwmFET_off MACRO 340 | IF FETON_DELAY != 0 341 | clr P1.ApwmFET 342 | ELSE 343 | setb P1.AcomFET 344 | ENDIF 345 | ENDM 346 | BpwmFET_on MACRO 347 | setb P1.BpwmFET 348 | IF FETON_DELAY == 0 349 | clr P1.BcomFET 350 | ENDIF 351 | ENDM 352 | BpwmFET_off MACRO 353 | IF FETON_DELAY != 0 354 | clr P1.BpwmFET 355 | ELSE 356 | setb P1.BcomFET 357 | ENDIF 358 | ENDM 359 | CpwmFET_on MACRO 360 | setb P1.CpwmFET 361 | IF FETON_DELAY == 0 362 | clr P1.CcomFET 363 | ENDIF 364 | ENDM 365 | CpwmFET_off MACRO 366 | IF FETON_DELAY != 0 367 | clr P1.CpwmFET 368 | ELSE 369 | setb P1.CcomFET 370 | ENDIF 371 | ENDM 372 | All_pwmFETs_Off MACRO 373 | IF FETON_DELAY != 0 374 | clr P1.ApwmFET 375 | clr P1.BpwmFET 376 | clr P1.CpwmFET 377 | ELSE 378 | setb P1.AcomFET 379 | setb P1.BcomFET 380 | setb P1.CcomFET 381 | ENDIF 382 | ENDM 383 | 384 | AcomFET_on MACRO 385 | IF FETON_DELAY == 0 386 | clr P1.ApwmFET 387 | ENDIF 388 | clr P1.AcomFET 389 | ENDM 390 | AcomFET_off MACRO 391 | setb P1.AcomFET 392 | ENDM 393 | BcomFET_on MACRO 394 | IF FETON_DELAY == 0 395 | clr P1.BpwmFET 396 | ENDIF 397 | clr P1.BcomFET 398 | ENDM 399 | BcomFET_off MACRO 400 | setb P1.BcomFET 401 | ENDM 402 | CcomFET_on MACRO 403 | IF FETON_DELAY == 0 404 | clr P1.CpwmFET 405 | ENDIF 406 | clr P1.CcomFET 407 | ENDM 408 | CcomFET_off MACRO 409 | setb P1.CcomFET 410 | ENDM 411 | All_comFETs_Off MACRO 412 | setb P1.AcomFET 413 | setb P1.BcomFET 414 | setb P1.CcomFET 415 | ENDM 416 | 417 | Set_Pwm_A MACRO 418 | IF FETON_DELAY == 0 419 | clr P1.AcomFET 420 | mov P1SKIP, #3Eh 421 | ELSE 422 | mov P1SKIP, #3Ch 423 | ENDIF 424 | ENDM 425 | Set_Pwm_B MACRO 426 | IF FETON_DELAY == 0 427 | clr P1.BcomFET 428 | mov P1SKIP, #3Bh 429 | ELSE 430 | mov P1SKIP, #33h 431 | ENDIF 432 | ENDM 433 | Set_Pwm_C MACRO 434 | IF FETON_DELAY == 0 435 | clr P1.CcomFET 436 | mov P1SKIP, #2Fh 437 | ELSE 438 | mov P1SKIP, #0Fh 439 | ENDIF 440 | ENDM 441 | Set_Pwms_Off MACRO 442 | mov P1SKIP, #3Fh 443 | ENDM 444 | 445 | Set_Comp_Phase_A MACRO 446 | mov CMP0MX, #23h ; Set comparator multiplexer to phase A 447 | ENDM 448 | Set_Comp_Phase_B MACRO 449 | mov CMP0MX, #03h ; Set comparator multiplexer to phase B 450 | ENDM 451 | Set_Comp_Phase_C MACRO 452 | mov CMP0MX, #13h ; Set comparator multiplexer to phase C 453 | ENDM 454 | Read_Comp_Out MACRO 455 | mov A, CMP0CN0 ; Read comparator output 456 | cpl A 457 | ENDM 458 | 459 | 460 | ;********************* 461 | ; PORT 2 definitions * 462 | ;********************* 463 | DebugPin EQU 0 ;o 464 | 465 | P2_PUSHPULL EQU (1 SHL DebugPin) 466 | 467 | 468 | ;********************** 469 | ; MCU specific macros * 470 | ;********************** 471 | Interrupt_Table_Definition MACRO 472 | CSEG AT 0 ; Code segment start 473 | jmp reset 474 | CSEG AT 03h ; Int0 interrupt 475 | jmp int0_int 476 | IF MCU_48MHZ == 1 477 | CSEG AT 0Bh ; Timer0 overflow interrupt 478 | jmp t0_int 479 | ENDIF 480 | CSEG AT 13h ; Int1 interrupt 481 | jmp int1_int 482 | CSEG AT 1Bh ; Timer1 overflow interrupt 483 | jmp t1_int 484 | CSEG AT 2Bh ; Timer2 overflow interrupt 485 | jmp t2_int 486 | CSEG AT 5Bh ; Pca interrupt 487 | jmp pca_int 488 | CSEG AT 73h ; Timer3 overflow/compare interrupt 489 | jmp t3_int 490 | ENDM 491 | 492 | Initialize_Xbar MACRO 493 | mov XBR2, #40h ; Xbar enabled 494 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 495 | ENDM 496 | 497 | Initialize_Comparator MACRO 498 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 499 | mov CMP0MD, #00h ; Comparator response time 100ns 500 | ENDM 501 | Initialize_Adc MACRO 502 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 503 | IF MCU_48MHZ == 0 504 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 505 | ELSE 506 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 507 | ENDIF 508 | mov ADC0MX, #10h ; Select temp sensor input 509 | mov ADC0CN0, #80h ; ADC enabled 510 | mov ADC0CN1, #01h ; Common mode buffer enabled 511 | ENDM 512 | Start_Adc MACRO 513 | mov ADC0CN0, #90h ; ADC start 514 | ENDM 515 | Read_Adc_Result MACRO 516 | mov Temp1, ADC0L 517 | mov Temp2, ADC0H 518 | ENDM 519 | Stop_Adc MACRO 520 | ENDM 521 | Set_RPM_Out MACRO 522 | ENDM 523 | Clear_RPM_Out MACRO 524 | ENDM 525 | Set_MCU_Clk_24MHz MACRO 526 | mov CLKSEL, #13h ; Set clock to 24MHz 527 | mov SFRPAGE, #10h 528 | mov PFE0CN, #00h ; Set flash timing for 24MHz 529 | mov SFRPAGE, #00h 530 | mov Clock_Set_At_48MHz, #0 531 | ENDM 532 | Set_MCU_Clk_48MHz MACRO 533 | mov SFRPAGE, #10h 534 | mov PFE0CN, #30h ; Set flash timing for 48MHz 535 | mov SFRPAGE, #00h 536 | mov CLKSEL, #03h ; Set clock to 48MHz 537 | mov Clock_Set_At_48MHz, #1 538 | ENDM 539 | Set_LED_0 MACRO 540 | ENDM 541 | Clear_LED_0 MACRO 542 | ENDM 543 | Set_LED_1 MACRO 544 | ENDM 545 | Clear_LED_1 MACRO 546 | ENDM 547 | Set_LED_2 MACRO 548 | ENDM 549 | Clear_LED_2 MACRO 550 | ENDM 551 | Set_LED_3 MACRO 552 | ENDM 553 | Clear_LED_3 MACRO 554 | ENDM 555 | -------------------------------------------------------------------------------- /src/P.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "P". 26 | ; X X RC MA CC MB MC X X Cc Bc Ac Cp Bp Ap X 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#P_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#P_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#P_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#P_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#P_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#P_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#P_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#P_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#P_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#P_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#P_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#P_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#P_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#P_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#P_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#P_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#P_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#P_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#P_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#P_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#P_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#P_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | 127 | ;********************* 128 | ; PORT 0 definitions * 129 | ;********************* 130 | ; EQU 7 ;i 131 | ; EQU 6 ;i 132 | Rcp_In EQU 5 ;i 133 | Mux_A EQU 4 ;i 134 | Comp_Com EQU 3 ;i 135 | Mux_B EQU 2 ;i 136 | Mux_C EQU 1 ;i 137 | ; EQU 0 ;i 138 | 139 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 140 | P0_INIT EQU 0FFh 141 | P0_PUSHPULL EQU 0 142 | P0_SKIP EQU 0FFh 143 | 144 | Get_Rcp_Capture_Values MACRO 145 | anl TCON, #0EFh ; Disable timer0 146 | mov Temp1, TL0 ; Get timer0 values 147 | mov Temp2, TH0 148 | IF MCU_48MHZ == 1 149 | mov Temp3, Timer0_X 150 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 151 | inc Temp3 ; If it is pending, then timer has already wrapped 152 | ENDIF 153 | mov TL0, #0 ; Reset timer0 154 | mov TH0, #0 155 | IF MCU_48MHZ == 1 156 | mov Timer0_X, #0 157 | ENDIF 158 | orl TCON, #10h ; Enable timer0 again 159 | IF MCU_48MHZ == 1 160 | mov A, Clock_Set_At_48MHz 161 | jnz Get_Rcp_End 162 | clr C 163 | mov A, Temp1 164 | rlc A 165 | mov Temp1, A 166 | mov A, Temp2 167 | rlc A 168 | mov Temp2, A 169 | mov A, Temp3 170 | rlc A 171 | mov Temp3, A 172 | Get_Rcp_End: 173 | ENDIF 174 | ENDM 175 | Decode_Dshot_2Msb MACRO 176 | movx A, @DPTR 177 | mov Temp6, A 178 | clr C 179 | subb A, Temp5 ; Subtract previous timestamp 180 | clr C 181 | subb A, Temp1 182 | jc t1_int_msb_fail ; Check that bit is longer than minimum 183 | 184 | subb A, Temp1 ; Check if bit is zero or one 185 | mov A, Temp4 ; Shift bit into data byte 186 | rlc A 187 | mov Temp4, A 188 | inc DPL ; Next bit 189 | movx A, @DPTR 190 | mov Temp5, A 191 | clr C 192 | subb A, Temp6 193 | clr C 194 | subb A, Temp1 195 | jc t1_int_msb_fail 196 | 197 | subb A, Temp1 198 | mov A, Temp4 199 | rlc A 200 | mov Temp4, A 201 | inc DPL 202 | ENDM 203 | Decode_Dshot_2Lsb MACRO 204 | movx A, @DPTR 205 | mov Temp6, A 206 | clr C 207 | subb A, Temp5 ; Subtract previous timestamp 208 | clr C 209 | subb A, Temp1 210 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 211 | 212 | subb A, Temp1 ; Check if bit is zero or one 213 | mov A, Temp3 ; Shift bit into data byte 214 | rlc A 215 | mov Temp3, A 216 | inc DPL ; Next bit 217 | movx A, @DPTR 218 | mov Temp5, A 219 | clr C 220 | subb A, Temp6 221 | clr C 222 | subb A, Temp1 223 | jc t1_int_lsb_fail 224 | 225 | subb A, Temp1 226 | mov A, Temp3 227 | rlc A 228 | mov Temp3, A 229 | inc DPL 230 | ENDM 231 | Initialize_PCA MACRO 232 | mov PCA0CN0, #40h ; PCA enabled 233 | mov PCA0MD, #08h ; PCA clock is system clock 234 | IF FETON_DELAY == 0 235 | IF MCU_48MHZ == 0 236 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 237 | ELSE 238 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 239 | ENDIF 240 | mov PCA0CENT, #00h ; Edge aligned pwm 241 | ELSE 242 | IF MCU_48MHZ == 0 243 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 244 | ELSE 245 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 246 | ENDIF 247 | mov PCA0CENT, #03h ; Center aligned pwm 248 | ENDIF 249 | ENDM 250 | Set_Pwm_Polarity MACRO 251 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 252 | ENDM 253 | Enable_Power_Pwm_Module MACRO 254 | IF FETON_DELAY == 0 255 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 256 | ELSE 257 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 258 | ENDIF 259 | ENDM 260 | Enable_Damp_Pwm_Module MACRO 261 | IF FETON_DELAY == 0 262 | mov PCA0CPM1, #00h ; Disable 263 | ELSE 264 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 265 | ENDIF 266 | ENDM 267 | Set_Power_Pwm_Regs MACRO 268 | IF FETON_DELAY == 0 269 | mov PCA0CPL0, Power_Pwm_Reg_L 270 | mov PCA0CPH0, Power_Pwm_Reg_H 271 | ELSE 272 | clr C 273 | mov A, Power_Pwm_Reg_H 274 | rrc A 275 | mov Temp1, A 276 | mov A, Power_Pwm_Reg_L 277 | rrc A 278 | mov PCA0CPL0, A 279 | mov PCA0CPH0, Temp1 280 | ENDIF 281 | ENDM 282 | Set_Damp_Pwm_Regs MACRO 283 | IF FETON_DELAY == 0 284 | mov PCA0CPL1, Damp_Pwm_Reg_L 285 | mov PCA0CPH1, Damp_Pwm_Reg_H 286 | ELSE 287 | clr C 288 | mov A, Damp_Pwm_Reg_H 289 | rrc A 290 | mov Temp1, A 291 | mov A, Damp_Pwm_Reg_L 292 | rrc A 293 | mov PCA0CPL1, A 294 | mov PCA0CPH1, Temp1 295 | ENDIF 296 | ENDM 297 | Clear_COVF_Interrupt MACRO 298 | anl PCA0PWM, #0DFh 299 | ENDM 300 | Clear_CCF_Interrupt MACRO 301 | anl PCA0CN0, #0FEh 302 | ENDM 303 | Enable_COVF_Interrupt MACRO 304 | orl PCA0PWM, #40h 305 | ENDM 306 | Enable_CCF_Interrupt MACRO 307 | orl PCA0CPM0,#01h 308 | ENDM 309 | Disable_COVF_Interrupt MACRO 310 | anl PCA0PWM, #0BFh 311 | ENDM 312 | Disable_CCF_Interrupt MACRO 313 | anl PCA0CPM0,#0FEh 314 | ENDM 315 | 316 | 317 | 318 | ;********************* 319 | ; PORT 1 definitions * 320 | ;********************* 321 | ; EQU 7 ;i 322 | CcomFET EQU 6 ;o 323 | BcomFET EQU 5 ;o 324 | AcomFET EQU 4 ;o 325 | CpwmFET EQU 3 ;i 326 | BpwmFET EQU 2 ;o 327 | ApwmFET EQU 1 ;o 328 | ; EQU 0 ;o 329 | 330 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 331 | P1_INIT EQU 00h 332 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 333 | P1_SKIP EQU 7Fh 334 | 335 | ApwmFET_on MACRO 336 | setb P1.ApwmFET 337 | IF FETON_DELAY == 0 338 | setb P1.AcomFET 339 | ENDIF 340 | ENDM 341 | ApwmFET_off MACRO 342 | IF FETON_DELAY != 0 343 | clr P1.ApwmFET 344 | ELSE 345 | clr P1.AcomFET 346 | ENDIF 347 | ENDM 348 | BpwmFET_on MACRO 349 | setb P1.BpwmFET 350 | IF FETON_DELAY == 0 351 | setb P1.BcomFET 352 | ENDIF 353 | ENDM 354 | BpwmFET_off MACRO 355 | IF FETON_DELAY != 0 356 | clr P1.BpwmFET 357 | ELSE 358 | clr P1.BcomFET 359 | ENDIF 360 | ENDM 361 | CpwmFET_on MACRO 362 | setb P1.CpwmFET 363 | IF FETON_DELAY == 0 364 | setb P1.CcomFET 365 | ENDIF 366 | ENDM 367 | CpwmFET_off MACRO 368 | IF FETON_DELAY != 0 369 | clr P1.CpwmFET 370 | ELSE 371 | clr P1.CcomFET 372 | ENDIF 373 | ENDM 374 | All_pwmFETs_Off MACRO 375 | IF FETON_DELAY != 0 376 | clr P1.ApwmFET 377 | clr P1.BpwmFET 378 | clr P1.CpwmFET 379 | ELSE 380 | clr P1.AcomFET 381 | clr P1.BcomFET 382 | clr P1.CcomFET 383 | ENDIF 384 | ENDM 385 | 386 | AcomFET_on MACRO 387 | IF FETON_DELAY == 0 388 | clr P1.ApwmFET 389 | ENDIF 390 | setb P1.AcomFET 391 | ENDM 392 | AcomFET_off MACRO 393 | clr P1.AcomFET 394 | ENDM 395 | BcomFET_on MACRO 396 | IF FETON_DELAY == 0 397 | clr P1.BpwmFET 398 | ENDIF 399 | setb P1.BcomFET 400 | ENDM 401 | BcomFET_off MACRO 402 | clr P1.BcomFET 403 | ENDM 404 | CcomFET_on MACRO 405 | IF FETON_DELAY == 0 406 | clr P1.CpwmFET 407 | ENDIF 408 | setb P1.CcomFET 409 | ENDM 410 | CcomFET_off MACRO 411 | clr P1.CcomFET 412 | ENDM 413 | All_comFETs_Off MACRO 414 | clr P1.AcomFET 415 | clr P1.BcomFET 416 | clr P1.CcomFET 417 | ENDM 418 | 419 | Set_Pwm_A MACRO 420 | IF FETON_DELAY == 0 421 | setb P1.AcomFET 422 | mov P1SKIP, #7Dh 423 | ELSE 424 | mov P1SKIP, #6Dh 425 | ENDIF 426 | ENDM 427 | Set_Pwm_B MACRO 428 | IF FETON_DELAY == 0 429 | setb P1.BcomFET 430 | mov P1SKIP, #7Bh 431 | ELSE 432 | mov P1SKIP, #5Bh 433 | ENDIF 434 | ENDM 435 | Set_Pwm_C MACRO 436 | IF FETON_DELAY == 0 437 | setb P1.CcomFET 438 | mov P1SKIP, #77h 439 | ELSE 440 | mov P1SKIP, #37h 441 | ENDIF 442 | ENDM 443 | Set_Pwms_Off MACRO 444 | mov P1SKIP, #7Fh 445 | ENDM 446 | 447 | Set_Comp_Phase_A MACRO 448 | mov CMP0MX, #43h ; Set comparator multiplexer to phase A 449 | ENDM 450 | Set_Comp_Phase_B MACRO 451 | mov CMP0MX, #23h ; Set comparator multiplexer to phase B 452 | ENDM 453 | Set_Comp_Phase_C MACRO 454 | mov CMP0MX, #13h ; Set comparator multiplexer to phase C 455 | ENDM 456 | Read_Comp_Out MACRO 457 | mov A, CMP0CN0 ; Read comparator output 458 | ENDM 459 | 460 | 461 | ;********************* 462 | ; PORT 2 definitions * 463 | ;********************* 464 | DebugPin EQU 0 ;o 465 | 466 | P2_PUSHPULL EQU (1 SHL DebugPin) 467 | 468 | 469 | ;********************** 470 | ; MCU specific macros * 471 | ;********************** 472 | Interrupt_Table_Definition MACRO 473 | CSEG AT 0 ; Code segment start 474 | jmp reset 475 | CSEG AT 03h ; Int0 interrupt 476 | jmp int0_int 477 | IF MCU_48MHZ == 1 478 | CSEG AT 0Bh ; Timer0 overflow interrupt 479 | jmp t0_int 480 | ENDIF 481 | CSEG AT 13h ; Int1 interrupt 482 | jmp int1_int 483 | CSEG AT 1Bh ; Timer1 overflow interrupt 484 | jmp t1_int 485 | CSEG AT 2Bh ; Timer2 overflow interrupt 486 | jmp t2_int 487 | CSEG AT 5Bh ; Pca interrupt 488 | jmp pca_int 489 | CSEG AT 73h ; Timer3 overflow/compare interrupt 490 | jmp t3_int 491 | ENDM 492 | 493 | Initialize_Xbar MACRO 494 | mov XBR2, #40h ; Xbar enabled 495 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 496 | ENDM 497 | 498 | Initialize_Comparator MACRO 499 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 500 | mov CMP0MD, #00h ; Comparator response time 100ns 501 | ENDM 502 | Initialize_Adc MACRO 503 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 504 | IF MCU_48MHZ == 0 505 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 506 | ELSE 507 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 508 | ENDIF 509 | mov ADC0MX, #10h ; Select temp sensor input 510 | mov ADC0CN0, #80h ; ADC enabled 511 | mov ADC0CN1, #01h ; Common mode buffer enabled 512 | ENDM 513 | Start_Adc MACRO 514 | mov ADC0CN0, #90h ; ADC start 515 | ENDM 516 | Read_Adc_Result MACRO 517 | mov Temp1, ADC0L 518 | mov Temp2, ADC0H 519 | ENDM 520 | Stop_Adc MACRO 521 | ENDM 522 | Set_RPM_Out MACRO 523 | ENDM 524 | Clear_RPM_Out MACRO 525 | ENDM 526 | Set_MCU_Clk_24MHz MACRO 527 | mov CLKSEL, #13h ; Set clock to 24MHz 528 | mov SFRPAGE, #10h 529 | mov PFE0CN, #00h ; Set flash timing for 24MHz 530 | mov SFRPAGE, #00h 531 | mov Clock_Set_At_48MHz, #0 532 | ENDM 533 | Set_MCU_Clk_48MHz MACRO 534 | mov SFRPAGE, #10h 535 | mov PFE0CN, #30h ; Set flash timing for 48MHz 536 | mov SFRPAGE, #00h 537 | mov CLKSEL, #03h ; Set clock to 48MHz 538 | mov Clock_Set_At_48MHz, #1 539 | ENDM 540 | Set_LED_0 MACRO 541 | ENDM 542 | Clear_LED_0 MACRO 543 | ENDM 544 | Set_LED_1 MACRO 545 | ENDM 546 | Clear_LED_1 MACRO 547 | ENDM 548 | Set_LED_2 MACRO 549 | ENDM 550 | Clear_LED_2 MACRO 551 | ENDM 552 | Set_LED_3 MACRO 553 | ENDM 554 | Clear_LED_3 MACRO 555 | ENDM 556 | -------------------------------------------------------------------------------- /src/R.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "R". 26 | ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#R_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#R_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#R_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#R_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#R_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#R_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#R_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#R_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#R_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#R_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#R_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#R_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#R_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#R_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#R_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#R_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#R_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#R_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#R_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#R_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#R_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#R_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 5 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | ;********************* 127 | ; PORT 0 definitions * 128 | ;********************* 129 | ; EQU 7 ;i 130 | ; EQU 6 ;i 131 | Rcp_In EQU 5 ;i 132 | ; EQU 4 ;i 133 | Mux_C EQU 3 ;i 134 | Mux_B EQU 2 ;i 135 | Mux_A EQU 1 ;i 136 | Comp_Com EQU 0 ;i 137 | 138 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 139 | P0_INIT EQU 0FFh 140 | P0_PUSHPULL EQU 0 141 | P0_SKIP EQU 0FFh 142 | 143 | Get_Rcp_Capture_Values MACRO 144 | anl TCON, #0EFh ; Disable timer0 145 | mov Temp1, TL0 ; Get timer0 values 146 | mov Temp2, TH0 147 | IF MCU_48MHZ == 1 148 | mov Temp3, Timer0_X 149 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 150 | inc Temp3 ; If it is pending, then timer has already wrapped 151 | ENDIF 152 | mov TL0, #0 ; Reset timer0 153 | mov TH0, #0 154 | IF MCU_48MHZ == 1 155 | mov Timer0_X, #0 156 | ENDIF 157 | orl TCON, #10h ; Enable timer0 again 158 | IF MCU_48MHZ == 1 159 | mov A, Clock_Set_At_48MHz 160 | jnz Get_Rcp_End 161 | clr C 162 | mov A, Temp1 163 | rlc A 164 | mov Temp1, A 165 | mov A, Temp2 166 | rlc A 167 | mov Temp2, A 168 | mov A, Temp3 169 | rlc A 170 | mov Temp3, A 171 | Get_Rcp_End: 172 | ENDIF 173 | ENDM 174 | Decode_Dshot_2Msb MACRO 175 | movx A, @DPTR 176 | mov Temp6, A 177 | clr C 178 | subb A, Temp5 ; Subtract previous timestamp 179 | clr C 180 | subb A, Temp1 181 | jc t1_int_msb_fail ; Check that bit is longer than minimum 182 | 183 | subb A, Temp1 ; Check if bit is zero or one 184 | mov A, Temp4 ; Shift bit into data byte 185 | rlc A 186 | mov Temp4, A 187 | inc DPL ; Next bit 188 | movx A, @DPTR 189 | mov Temp5, A 190 | clr C 191 | subb A, Temp6 192 | clr C 193 | subb A, Temp1 194 | jc t1_int_msb_fail 195 | 196 | subb A, Temp1 197 | mov A, Temp4 198 | rlc A 199 | mov Temp4, A 200 | inc DPL 201 | ENDM 202 | Decode_Dshot_2Lsb MACRO 203 | movx A, @DPTR 204 | mov Temp6, A 205 | clr C 206 | subb A, Temp5 ; Subtract previous timestamp 207 | clr C 208 | subb A, Temp1 209 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 210 | 211 | subb A, Temp1 ; Check if bit is zero or one 212 | mov A, Temp3 ; Shift bit into data byte 213 | rlc A 214 | mov Temp3, A 215 | inc DPL ; Next bit 216 | movx A, @DPTR 217 | mov Temp5, A 218 | clr C 219 | subb A, Temp6 220 | clr C 221 | subb A, Temp1 222 | jc t1_int_lsb_fail 223 | 224 | subb A, Temp1 225 | mov A, Temp3 226 | rlc A 227 | mov Temp3, A 228 | inc DPL 229 | ENDM 230 | Initialize_PCA MACRO 231 | mov PCA0CN0, #40h ; PCA enabled 232 | mov PCA0MD, #08h ; PCA clock is system clock 233 | IF FETON_DELAY == 0 234 | IF MCU_48MHZ == 0 235 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 236 | ELSE 237 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 238 | ENDIF 239 | mov PCA0CENT, #00h ; Edge aligned pwm 240 | ELSE 241 | IF MCU_48MHZ == 0 242 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 243 | ELSE 244 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 245 | ENDIF 246 | mov PCA0CENT, #03h ; Center aligned pwm 247 | ENDIF 248 | ENDM 249 | Set_Pwm_Polarity MACRO 250 | mov PCA0POL, #02h ; Damping inverted, pwm noninverted 251 | ENDM 252 | Enable_Power_Pwm_Module MACRO 253 | IF FETON_DELAY == 0 254 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 255 | ELSE 256 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 257 | ENDIF 258 | ENDM 259 | Enable_Damp_Pwm_Module MACRO 260 | IF FETON_DELAY == 0 261 | mov PCA0CPM1, #00h ; Disable 262 | ELSE 263 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 264 | ENDIF 265 | ENDM 266 | Set_Power_Pwm_Regs MACRO 267 | IF FETON_DELAY == 0 268 | mov PCA0CPL0, Power_Pwm_Reg_L 269 | mov PCA0CPH0, Power_Pwm_Reg_H 270 | ELSE 271 | clr C 272 | mov A, Power_Pwm_Reg_H 273 | rrc A 274 | mov Temp1, A 275 | mov A, Power_Pwm_Reg_L 276 | rrc A 277 | mov PCA0CPL0, A 278 | mov PCA0CPH0, Temp1 279 | ENDIF 280 | ENDM 281 | Set_Damp_Pwm_Regs MACRO 282 | IF FETON_DELAY == 0 283 | mov PCA0CPL1, Damp_Pwm_Reg_L 284 | mov PCA0CPH1, Damp_Pwm_Reg_H 285 | ELSE 286 | clr C 287 | mov A, Damp_Pwm_Reg_H 288 | rrc A 289 | mov Temp1, A 290 | mov A, Damp_Pwm_Reg_L 291 | rrc A 292 | mov PCA0CPL1, A 293 | mov PCA0CPH1, Temp1 294 | ENDIF 295 | ENDM 296 | Clear_COVF_Interrupt MACRO 297 | anl PCA0PWM, #0DFh 298 | ENDM 299 | Clear_CCF_Interrupt MACRO ; CCF interrupt is only used for FETON_DELAY == 0 300 | anl PCA0CN0, #0FEh 301 | ENDM 302 | Enable_COVF_Interrupt MACRO 303 | orl PCA0PWM, #40h 304 | ENDM 305 | Enable_CCF_Interrupt MACRO 306 | orl PCA0CPM0,#01h 307 | ENDM 308 | Disable_COVF_Interrupt MACRO 309 | anl PCA0PWM, #0BFh 310 | ENDM 311 | Disable_CCF_Interrupt MACRO 312 | anl PCA0CPM0,#0FEh 313 | ENDM 314 | 315 | ;********************* 316 | ; PORT 1 definitions * 317 | ;********************* 318 | ; EQU 7 ;i 319 | ; EQU 6 ;i 320 | AcomFET EQU 5 ;o 321 | BcomFET EQU 4 ;o 322 | CcomFET EQU 3 ;o 323 | ApwmFET EQU 2 ;o 324 | BpwmFET EQU 1 ;o 325 | CpwmFET EQU 0 ;o 326 | 327 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 328 | P1_INIT EQU 00h 329 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 330 | P1_SKIP EQU 3Fh 331 | 332 | ApwmFET_on MACRO 333 | setb P1.ApwmFET 334 | IF FETON_DELAY == 0 335 | setb P1.AcomFET 336 | ENDIF 337 | ENDM 338 | ApwmFET_off MACRO 339 | IF FETON_DELAY != 0 340 | clr P1.ApwmFET 341 | ELSE 342 | clr P1.AcomFET 343 | ENDIF 344 | ENDM 345 | BpwmFET_on MACRO 346 | setb P1.BpwmFET 347 | IF FETON_DELAY == 0 348 | setb P1.BcomFET 349 | ENDIF 350 | ENDM 351 | BpwmFET_off MACRO 352 | IF FETON_DELAY != 0 353 | clr P1.BpwmFET 354 | ELSE 355 | clr P1.BcomFET 356 | ENDIF 357 | ENDM 358 | CpwmFET_on MACRO 359 | setb P1.CpwmFET 360 | IF FETON_DELAY == 0 361 | setb P1.CcomFET 362 | ENDIF 363 | ENDM 364 | CpwmFET_off MACRO 365 | IF FETON_DELAY != 0 366 | clr P1.CpwmFET 367 | ELSE 368 | clr P1.CcomFET 369 | ENDIF 370 | ENDM 371 | All_pwmFETs_Off MACRO 372 | IF FETON_DELAY != 0 373 | clr P1.ApwmFET 374 | clr P1.BpwmFET 375 | clr P1.CpwmFET 376 | ELSE 377 | clr P1.AcomFET 378 | clr P1.BcomFET 379 | clr P1.CcomFET 380 | ENDIF 381 | ENDM 382 | 383 | AcomFET_on MACRO 384 | IF FETON_DELAY == 0 385 | clr P1.ApwmFET 386 | ENDIF 387 | setb P1.AcomFET 388 | ENDM 389 | AcomFET_off MACRO 390 | clr P1.AcomFET 391 | ENDM 392 | BcomFET_on MACRO 393 | IF FETON_DELAY == 0 394 | clr P1.BpwmFET 395 | ENDIF 396 | setb P1.BcomFET 397 | ENDM 398 | BcomFET_off MACRO 399 | clr P1.BcomFET 400 | ENDM 401 | CcomFET_on MACRO 402 | IF FETON_DELAY == 0 403 | clr P1.CpwmFET 404 | ENDIF 405 | setb P1.CcomFET 406 | ENDM 407 | CcomFET_off MACRO 408 | clr P1.CcomFET 409 | ENDM 410 | All_comFETs_Off MACRO 411 | clr P1.AcomFET 412 | clr P1.BcomFET 413 | clr P1.CcomFET 414 | ENDM 415 | 416 | Set_Pwm_A MACRO 417 | IF FETON_DELAY == 0 418 | setb P1.AcomFET 419 | mov P1SKIP, #3Bh 420 | ELSE 421 | mov P1SKIP, #1Bh 422 | ENDIF 423 | ENDM 424 | Set_Pwm_B MACRO 425 | IF FETON_DELAY == 0 426 | setb P1.BcomFET 427 | mov P1SKIP, #3Dh 428 | ELSE 429 | mov P1SKIP, #2Dh 430 | ENDIF 431 | ENDM 432 | Set_Pwm_C MACRO 433 | IF FETON_DELAY == 0 434 | setb P1.CcomFET 435 | mov P1SKIP, #3Eh 436 | ELSE 437 | mov P1SKIP, #36h 438 | ENDIF 439 | ENDM 440 | Set_Pwms_Off MACRO 441 | mov P1SKIP, #3Fh 442 | ENDM 443 | 444 | Set_Comp_Phase_A MACRO 445 | mov CMP0MX, #10h ; Set comparator multiplexer to phase A 446 | ENDM 447 | Set_Comp_Phase_B MACRO 448 | mov CMP0MX, #20h ; Set comparator multiplexer to phase B 449 | ENDM 450 | Set_Comp_Phase_C MACRO 451 | mov CMP0MX, #30h ; Set comparator multiplexer to phase C 452 | ENDM 453 | Read_Comp_Out MACRO 454 | mov A, CMP0CN0 ; Read comparator output 455 | ENDM 456 | 457 | 458 | ;********************* 459 | ; PORT 2 definitions * 460 | ;********************* 461 | DebugPin EQU 0 ;o 462 | 463 | P2_PUSHPULL EQU (1 SHL DebugPin) 464 | 465 | 466 | ;********************** 467 | ; MCU specific macros * 468 | ;********************** 469 | Interrupt_Table_Definition MACRO 470 | CSEG AT 0 ; Code segment start 471 | jmp reset 472 | CSEG AT 03h ; Int0 interrupt 473 | jmp int0_int 474 | IF MCU_48MHZ == 1 475 | CSEG AT 0Bh ; Timer0 overflow interrupt 476 | jmp t0_int 477 | ENDIF 478 | CSEG AT 13h ; Int1 interrupt 479 | jmp int1_int 480 | CSEG AT 1Bh ; Timer1 overflow interrupt 481 | jmp t1_int 482 | CSEG AT 2Bh ; Timer2 overflow interrupt 483 | jmp t2_int 484 | CSEG AT 5Bh ; Pca interrupt 485 | jmp pca_int 486 | CSEG AT 73h ; Timer3 overflow/compare interrupt 487 | jmp t3_int 488 | ENDM 489 | 490 | Initialize_Xbar MACRO 491 | mov XBR2, #40h ; Xbar enabled 492 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 493 | ENDM 494 | 495 | Initialize_Comparator MACRO 496 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 497 | mov CMP0MD, #00h ; Comparator response time 100ns 498 | ENDM 499 | Initialize_Adc MACRO 500 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 501 | IF MCU_48MHZ == 0 502 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 503 | ELSE 504 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 505 | ENDIF 506 | mov ADC0MX, #10h ; Select temp sensor input 507 | mov ADC0CN0, #80h ; ADC enabled 508 | mov ADC0CN1, #01h ; Common mode buffer enabled 509 | ENDM 510 | Start_Adc MACRO 511 | mov ADC0CN0, #90h ; ADC start 512 | ENDM 513 | Read_Adc_Result MACRO 514 | mov Temp1, ADC0L 515 | mov Temp2, ADC0H 516 | ENDM 517 | Stop_Adc MACRO 518 | ENDM 519 | Set_RPM_Out MACRO 520 | ENDM 521 | Clear_RPM_Out MACRO 522 | ENDM 523 | Set_MCU_Clk_24MHz MACRO 524 | mov CLKSEL, #13h ; Set clock to 24MHz 525 | mov SFRPAGE, #10h 526 | mov PFE0CN, #00h ; Set flash timing for 24MHz 527 | mov SFRPAGE, #00h 528 | mov Clock_Set_At_48MHz, #0 529 | ENDM 530 | Set_MCU_Clk_48MHz MACRO 531 | mov SFRPAGE, #10h 532 | mov PFE0CN, #30h ; Set flash timing for 48MHz 533 | mov SFRPAGE, #00h 534 | mov CLKSEL, #03h ; Set clock to 48MHz 535 | mov Clock_Set_At_48MHz, #1 536 | ENDM 537 | Set_LED_0 MACRO 538 | ENDM 539 | Clear_LED_0 MACRO 540 | ENDM 541 | Set_LED_1 MACRO 542 | ENDM 543 | Clear_LED_1 MACRO 544 | ENDM 545 | Set_LED_2 MACRO 546 | ENDM 547 | Clear_LED_2 MACRO 548 | ENDM 549 | Set_LED_3 MACRO 550 | ENDM 551 | Clear_LED_3 MACRO 552 | ENDM 553 | -------------------------------------------------------------------------------- /src/T.inc: -------------------------------------------------------------------------------- 1 | ;**** **** **** **** **** 2 | ; 3 | ; BLHeli program for controlling brushless motors in helicopters and multirotors 4 | ; 5 | ; Copyright 2011, 2012 Steffen Skaug 6 | ; This program is distributed under the terms of the GNU General Public License 7 | ; 8 | ; This file is part of BLHeli. 9 | ; 10 | ; BLHeli is free software: you can redistribute it and/or modify 11 | ; it under the terms of the GNU General Public License as published by 12 | ; the Free Software Foundation, either version 3 of the License, or 13 | ; (at your option) any later version. 14 | ; 15 | ; BLHeli is distributed in the hope that it will be useful, 16 | ; but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | ; GNU General Public License for more details. 19 | ; 20 | ; You should have received a copy of the GNU General Public License 21 | ; along with BLHeli. If not, see . 22 | ; 23 | ;**** **** **** **** **** 24 | ; 25 | ; Hardware definition file "T". 26 | ; RC X MA X MB CC MC X X X Cp Bp Ap Ac Bc Cc 27 | ; 28 | ;**** **** **** **** **** 29 | 30 | 31 | 32 | ;********************* 33 | ; Device SiLabs EFM8BB1x/2x 34 | ;********************* 35 | IF MCU_48MHZ == 0 36 | $include (SI_EFM8BB1_Defs.inc) 37 | ELSE 38 | $include (SI_EFM8BB2_Defs.inc) 39 | ENDIF 40 | 41 | 42 | ;**** **** **** **** **** 43 | ; Uses internal calibrated oscillator set to 24/48Mhz 44 | ;**** **** **** **** **** 45 | 46 | ;**** **** **** **** **** 47 | ; Constant definitions 48 | ;**** **** **** **** **** 49 | IF MCU_48MHZ == 0 50 | CSEG AT 1A40h 51 | IF FETON_DELAY == 0 52 | Eep_ESC_Layout: DB "#T_L_00# " ; ESC layout tag 53 | ELSEIF FETON_DELAY == 5 54 | Eep_ESC_Layout: DB "#T_L_05# " 55 | ELSEIF FETON_DELAY == 10 56 | Eep_ESC_Layout: DB "#T_L_10# " 57 | ELSEIF FETON_DELAY == 15 58 | Eep_ESC_Layout: DB "#T_L_15# " 59 | ELSEIF FETON_DELAY == 20 60 | Eep_ESC_Layout: DB "#T_L_20# " 61 | ELSEIF FETON_DELAY == 25 62 | Eep_ESC_Layout: DB "#T_L_25# " 63 | ELSEIF FETON_DELAY == 30 64 | Eep_ESC_Layout: DB "#T_L_30# " 65 | ELSEIF FETON_DELAY == 40 66 | Eep_ESC_Layout: DB "#T_L_40# " 67 | ELSEIF FETON_DELAY == 50 68 | Eep_ESC_Layout: DB "#T_L_50# " 69 | ELSEIF FETON_DELAY == 70 70 | Eep_ESC_Layout: DB "#T_L_70# " 71 | ELSEIF FETON_DELAY == 90 72 | Eep_ESC_Layout: DB "#T_L_90# " 73 | ENDIF 74 | CSEG AT 1A50h 75 | Eep_ESC_MCU: DB "#BLHELI$EFM8B10#" ; Project and MCU tag (16 Bytes) 76 | 77 | ELSE 78 | 79 | CSEG AT 1A40h 80 | IF FETON_DELAY == 0 81 | Eep_ESC_Layout: DB "#T_H_00# " ; ESC layout tag 82 | ELSEIF FETON_DELAY == 5 83 | Eep_ESC_Layout: DB "#T_H_05# " 84 | ELSEIF FETON_DELAY == 10 85 | Eep_ESC_Layout: DB "#T_H_10# " 86 | ELSEIF FETON_DELAY == 15 87 | Eep_ESC_Layout: DB "#T_H_15# " 88 | ELSEIF FETON_DELAY == 20 89 | Eep_ESC_Layout: DB "#T_H_20# " 90 | ELSEIF FETON_DELAY == 25 91 | Eep_ESC_Layout: DB "#T_H_25# " 92 | ELSEIF FETON_DELAY == 30 93 | Eep_ESC_Layout: DB "#T_H_30# " 94 | ELSEIF FETON_DELAY == 40 95 | Eep_ESC_Layout: DB "#T_H_40# " 96 | ELSEIF FETON_DELAY == 50 97 | Eep_ESC_Layout: DB "#T_H_50# " 98 | ELSEIF FETON_DELAY == 70 99 | Eep_ESC_Layout: DB "#T_H_70# " 100 | ELSEIF FETON_DELAY == 90 101 | Eep_ESC_Layout: DB "#T_H_90# " 102 | ENDIF 103 | CSEG AT 1A50h 104 | Eep_ESC_MCU: DB "#BLHELI$EFM8B21#" ; Project and MCU tag (16 Bytes) 105 | ENDIF 106 | 107 | TEMP_LIMIT EQU 49 ; Temperature measurement ADC value for which main motor power is limited at 80degC (low byte, assuming high byte is 1) 108 | TEMP_LIMIT_STEP EQU 9 ; Temperature measurement ADC value increment for another 10degC 109 | 110 | ;**** **** **** **** **** 111 | ; Bootloader definitions 112 | ;**** **** **** **** **** 113 | RTX_PORT EQU P0 ; Receive/Transmit port 114 | RTX_MDOUT EQU P0MDOUT ; Set to 1 for PUSHPULL 115 | RTX_MDIN EQU P0MDIN ; Set to 1 for DIGITAL 116 | RTX_SKIP EQU P0SKIP ; Set to 1 for SKIP 117 | RTX_PIN EQU 7 ; RTX pin 118 | 119 | SIGNATURE_001 EQU 0E8h ; Device signature 120 | IF MCU_48MHZ == 0 121 | SIGNATURE_002 EQU 0B1h 122 | ELSE 123 | SIGNATURE_002 EQU 0B2h 124 | ENDIF 125 | 126 | 127 | ;********************* 128 | ; PORT 0 definitions * 129 | ;********************* 130 | Rcp_In EQU 7 ;i 131 | ; EQU 6 ;i 132 | Mux_A EQU 5 ;i 133 | ; EQU 4 ;i 134 | Mux_B EQU 3 ;i 135 | Comp_Com EQU 2 ;i 136 | Mux_C EQU 1 ;i 137 | ; EQU 0 ;i 138 | 139 | P0_DIGITAL EQU NOT((1 SHL Mux_A)+(1 SHL Mux_B)+(1 SHL Mux_C)+(1 SHL Comp_Com)) 140 | P0_INIT EQU 0FFh 141 | P0_PUSHPULL EQU 0 142 | P0_SKIP EQU 0FFh 143 | 144 | Get_Rcp_Capture_Values MACRO 145 | anl TCON, #0EFh ; Disable timer0 146 | mov Temp1, TL0 ; Get timer0 values 147 | mov Temp2, TH0 148 | IF MCU_48MHZ == 1 149 | mov Temp3, Timer0_X 150 | jnb TCON_TF0, ($+4) ; Check if interrupt is pending 151 | inc Temp3 ; If it is pending, then timer has already wrapped 152 | ENDIF 153 | mov TL0, #0 ; Reset timer0 154 | mov TH0, #0 155 | IF MCU_48MHZ == 1 156 | mov Timer0_X, #0 157 | ENDIF 158 | orl TCON, #10h ; Enable timer0 again 159 | IF MCU_48MHZ == 1 160 | mov A, Clock_Set_At_48MHz 161 | jnz Get_Rcp_End 162 | clr C 163 | mov A, Temp1 164 | rlc A 165 | mov Temp1, A 166 | mov A, Temp2 167 | rlc A 168 | mov Temp2, A 169 | mov A, Temp3 170 | rlc A 171 | mov Temp3, A 172 | Get_Rcp_End: 173 | ENDIF 174 | ENDM 175 | Decode_Dshot_2Msb MACRO 176 | movx A, @DPTR 177 | mov Temp6, A 178 | clr C 179 | subb A, Temp5 ; Subtract previous timestamp 180 | clr C 181 | subb A, Temp1 182 | jc t1_int_msb_fail ; Check that bit is longer than minimum 183 | 184 | subb A, Temp1 ; Check if bit is zero or one 185 | mov A, Temp4 ; Shift bit into data byte 186 | rlc A 187 | mov Temp4, A 188 | inc DPL ; Next bit 189 | movx A, @DPTR 190 | mov Temp5, A 191 | clr C 192 | subb A, Temp6 193 | clr C 194 | subb A, Temp1 195 | jc t1_int_msb_fail 196 | 197 | subb A, Temp1 198 | mov A, Temp4 199 | rlc A 200 | mov Temp4, A 201 | inc DPL 202 | ENDM 203 | Decode_Dshot_2Lsb MACRO 204 | movx A, @DPTR 205 | mov Temp6, A 206 | clr C 207 | subb A, Temp5 ; Subtract previous timestamp 208 | clr C 209 | subb A, Temp1 210 | jc t1_int_lsb_fail ; Check that bit is longer than minimum 211 | 212 | subb A, Temp1 ; Check if bit is zero or one 213 | mov A, Temp3 ; Shift bit into data byte 214 | rlc A 215 | mov Temp3, A 216 | inc DPL ; Next bit 217 | movx A, @DPTR 218 | mov Temp5, A 219 | clr C 220 | subb A, Temp6 221 | clr C 222 | subb A, Temp1 223 | jc t1_int_lsb_fail 224 | 225 | subb A, Temp1 226 | mov A, Temp3 227 | rlc A 228 | mov Temp3, A 229 | inc DPL 230 | ENDM 231 | Initialize_PCA MACRO 232 | mov PCA0CN0, #40h ; PCA enabled 233 | mov PCA0MD, #08h ; PCA clock is system clock 234 | IF FETON_DELAY == 0 235 | IF MCU_48MHZ == 0 236 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 237 | ELSE 238 | mov PCA0PWM, #83h ; PCA ARSEL set and 11bits pwm 239 | ENDIF 240 | mov PCA0CENT, #00h ; Edge aligned pwm 241 | ELSE 242 | IF MCU_48MHZ == 0 243 | mov PCA0PWM, #81h ; PCA ARSEL set and 9bits pwm 244 | ELSE 245 | mov PCA0PWM, #82h ; PCA ARSEL set and 10bits pwm 246 | ENDIF 247 | mov PCA0CENT, #03h ; Center aligned pwm 248 | ENDIF 249 | ENDM 250 | Set_Pwm_Polarity MACRO 251 | IF FETON_DELAY == 0 252 | mov PCA0POL, #00h ; Pwm noninverted 253 | ELSE 254 | mov PCA0POL, #01h ; Damping inverted, pwm noninverted 255 | ENDIF 256 | ENDM 257 | Enable_Power_Pwm_Module MACRO 258 | IF FETON_DELAY == 0 259 | mov PCA0CPM0, #4Ah ; Enable comparator of module, enable match, set pwm mode 260 | ELSE 261 | mov PCA0CPM1, #42h ; Enable comparator of module, set pwm mode 262 | ENDIF 263 | ENDM 264 | Enable_Damp_Pwm_Module MACRO 265 | IF FETON_DELAY == 0 266 | mov PCA0CPM1, #00h ; Disable 267 | ELSE 268 | mov PCA0CPM0, #42h ; Enable comparator of module, set pwm mode 269 | ENDIF 270 | ENDM 271 | Set_Power_Pwm_Regs MACRO 272 | IF FETON_DELAY == 0 273 | mov PCA0CPL0, Power_Pwm_Reg_L 274 | mov PCA0CPH0, Power_Pwm_Reg_H 275 | ELSE 276 | clr C 277 | mov A, Power_Pwm_Reg_H 278 | rrc A 279 | mov Temp1, A 280 | mov A, Power_Pwm_Reg_L 281 | rrc A 282 | mov PCA0CPL1, A 283 | mov PCA0CPH1, Temp1 284 | ENDIF 285 | ENDM 286 | Set_Damp_Pwm_Regs MACRO 287 | IF FETON_DELAY == 0 288 | mov PCA0CPL1, Damp_Pwm_Reg_L 289 | mov PCA0CPH1, Damp_Pwm_Reg_H 290 | ELSE 291 | clr C 292 | mov A, Damp_Pwm_Reg_H 293 | rrc A 294 | mov Temp1, A 295 | mov A, Damp_Pwm_Reg_L 296 | rrc A 297 | mov PCA0CPL0, A 298 | mov PCA0CPH0, Temp1 299 | ENDIF 300 | ENDM 301 | Clear_COVF_Interrupt MACRO 302 | anl PCA0PWM, #0DFh 303 | ENDM 304 | Clear_CCF_Interrupt MACRO ; CCF interrupt is only used for FETON_DELAY == 0 305 | anl PCA0CN0, #0FEh 306 | ENDM 307 | Enable_COVF_Interrupt MACRO 308 | orl PCA0PWM, #40h 309 | ENDM 310 | Enable_CCF_Interrupt MACRO 311 | orl PCA0CPM0,#01h 312 | ENDM 313 | Disable_COVF_Interrupt MACRO 314 | anl PCA0PWM, #0BFh 315 | ENDM 316 | Disable_CCF_Interrupt MACRO 317 | anl PCA0CPM0,#0FEh 318 | ENDM 319 | 320 | 321 | ;********************* 322 | ; PORT 1 definitions * 323 | ;********************* 324 | ; EQU 7 ;i 325 | ; EQU 6 ;i 326 | CpwmFET EQU 5 ;o 327 | BpwmFET EQU 4 ;o 328 | ApwmFET EQU 3 ;o 329 | AcomFET EQU 2 ;o 330 | BcomFET EQU 1 ;o 331 | CcomFET EQU 0 ;o 332 | 333 | P1_DIGITAL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 334 | P1_INIT EQU 00h 335 | P1_PUSHPULL EQU (1 SHL ApwmFET)+(1 SHL BpwmFET)+(1 SHL CpwmFET)+(1 SHL AcomFET)+(1 SHL BcomFET)+(1 SHL CcomFET) 336 | P1_SKIP EQU 3Fh 337 | 338 | ApwmFET_on MACRO 339 | setb P1.ApwmFET 340 | IF FETON_DELAY == 0 341 | setb P1.AcomFET 342 | ENDIF 343 | ENDM 344 | ApwmFET_off MACRO 345 | IF FETON_DELAY != 0 346 | clr P1.ApwmFET 347 | ELSE 348 | clr P1.AcomFET 349 | ENDIF 350 | ENDM 351 | BpwmFET_on MACRO 352 | setb P1.BpwmFET 353 | IF FETON_DELAY == 0 354 | setb P1.BcomFET 355 | ENDIF 356 | ENDM 357 | BpwmFET_off MACRO 358 | IF FETON_DELAY != 0 359 | clr P1.BpwmFET 360 | ELSE 361 | clr P1.BcomFET 362 | ENDIF 363 | ENDM 364 | CpwmFET_on MACRO 365 | setb P1.CpwmFET 366 | IF FETON_DELAY == 0 367 | setb P1.CcomFET 368 | ENDIF 369 | ENDM 370 | CpwmFET_off MACRO 371 | IF FETON_DELAY != 0 372 | clr P1.CpwmFET 373 | ELSE 374 | clr P1.CcomFET 375 | ENDIF 376 | ENDM 377 | All_pwmFETs_Off MACRO 378 | IF FETON_DELAY != 0 379 | clr P1.ApwmFET 380 | clr P1.BpwmFET 381 | clr P1.CpwmFET 382 | ELSE 383 | clr P1.AcomFET 384 | clr P1.BcomFET 385 | clr P1.CcomFET 386 | ENDIF 387 | ENDM 388 | 389 | AcomFET_on MACRO 390 | IF FETON_DELAY == 0 391 | clr P1.ApwmFET 392 | ENDIF 393 | setb P1.AcomFET 394 | ENDM 395 | AcomFET_off MACRO 396 | clr P1.AcomFET 397 | ENDM 398 | BcomFET_on MACRO 399 | IF FETON_DELAY == 0 400 | clr P1.BpwmFET 401 | ENDIF 402 | setb P1.BcomFET 403 | ENDM 404 | BcomFET_off MACRO 405 | clr P1.BcomFET 406 | ENDM 407 | CcomFET_on MACRO 408 | IF FETON_DELAY == 0 409 | clr P1.CpwmFET 410 | ENDIF 411 | setb P1.CcomFET 412 | ENDM 413 | CcomFET_off MACRO 414 | clr P1.CcomFET 415 | ENDM 416 | All_comFETs_Off MACRO 417 | clr P1.AcomFET 418 | clr P1.BcomFET 419 | clr P1.CcomFET 420 | ENDM 421 | 422 | Set_Pwm_A MACRO 423 | IF FETON_DELAY == 0 424 | setb P1.AcomFET 425 | mov P1SKIP, #37h 426 | ELSE 427 | mov P1SKIP, #33h 428 | ENDIF 429 | ENDM 430 | Set_Pwm_B MACRO 431 | IF FETON_DELAY == 0 432 | setb P1.BcomFET 433 | mov P1SKIP, #2Fh 434 | ELSE 435 | mov P1SKIP, #2Dh 436 | ENDIF 437 | ENDM 438 | Set_Pwm_C MACRO 439 | IF FETON_DELAY == 0 440 | setb P1.CcomFET 441 | mov P1SKIP, #1Fh 442 | ELSE 443 | mov P1SKIP, #1Eh 444 | ENDIF 445 | ENDM 446 | Set_Pwms_Off MACRO 447 | mov P1SKIP, #3Fh 448 | ENDM 449 | 450 | Set_Comp_Phase_A MACRO 451 | mov CMP0MX, #52h ; Set comparator multiplexer to phase A 452 | ENDM 453 | Set_Comp_Phase_B MACRO 454 | mov CMP0MX, #32h ; Set comparator multiplexer to phase B 455 | ENDM 456 | Set_Comp_Phase_C MACRO 457 | mov CMP0MX, #12h ; Set comparator multiplexer to phase C 458 | ENDM 459 | Read_Comp_Out MACRO 460 | mov A, CMP0CN0 ; Read comparator output 461 | ENDM 462 | 463 | 464 | ;********************* 465 | ; PORT 2 definitions * 466 | ;********************* 467 | DebugPin EQU 0 ;o 468 | 469 | P2_PUSHPULL EQU (1 SHL DebugPin) 470 | 471 | 472 | ;********************** 473 | ; MCU specific macros * 474 | ;********************** 475 | Interrupt_Table_Definition MACRO 476 | CSEG AT 0 ; Code segment start 477 | jmp reset 478 | CSEG AT 03h ; Int0 interrupt 479 | jmp int0_int 480 | IF MCU_48MHZ == 1 481 | CSEG AT 0Bh ; Timer0 overflow interrupt 482 | jmp t0_int 483 | ENDIF 484 | CSEG AT 13h ; Int1 interrupt 485 | jmp int1_int 486 | CSEG AT 1Bh ; Timer1 overflow interrupt 487 | jmp t1_int 488 | CSEG AT 2Bh ; Timer2 overflow interrupt 489 | jmp t2_int 490 | CSEG AT 5Bh ; Pca interrupt 491 | jmp pca_int 492 | CSEG AT 73h ; Timer3 overflow/compare interrupt 493 | jmp t3_int 494 | ENDM 495 | 496 | Initialize_Xbar MACRO 497 | mov XBR2, #40h ; Xbar enabled 498 | mov XBR1, #02h ; CEX0 and CEX1 routed to pins 499 | ENDM 500 | 501 | Initialize_Comparator MACRO 502 | mov CMP0CN0, #80h ; Comparator enabled, no hysteresis 503 | mov CMP0MD, #00h ; Comparator response time 100ns 504 | ENDM 505 | Initialize_Adc MACRO 506 | mov REF0CN, #0Ch ; Set vdd (3.3V) as reference. Enable temp sensor and bias 507 | IF MCU_48MHZ == 0 508 | mov ADC0CF, #59h ; ADC clock 2MHz, PGA gain 1 509 | ELSE 510 | mov ADC0CF, #0B9h ; ADC clock 2MHz, PGA gain 1 511 | ENDIF 512 | mov ADC0MX, #10h ; Select temp sensor input 513 | mov ADC0CN0, #80h ; ADC enabled 514 | mov ADC0CN1, #01h ; Common mode buffer enabled 515 | ENDM 516 | Start_Adc MACRO 517 | mov ADC0CN0, #90h ; ADC start 518 | ENDM 519 | Read_Adc_Result MACRO 520 | mov Temp1, ADC0L 521 | mov Temp2, ADC0H 522 | ENDM 523 | Stop_Adc MACRO 524 | ENDM 525 | Set_RPM_Out MACRO 526 | ENDM 527 | Clear_RPM_Out MACRO 528 | ENDM 529 | Set_MCU_Clk_24MHz MACRO 530 | mov CLKSEL, #13h ; Set clock to 24MHz 531 | mov SFRPAGE, #10h 532 | mov PFE0CN, #00h ; Set flash timing for 24MHz 533 | mov SFRPAGE, #00h 534 | mov Clock_Set_At_48MHz, #0 535 | ENDM 536 | Set_MCU_Clk_48MHz MACRO 537 | mov SFRPAGE, #10h 538 | mov PFE0CN, #30h ; Set flash timing for 48MHz 539 | mov SFRPAGE, #00h 540 | mov CLKSEL, #03h ; Set clock to 48MHz 541 | mov Clock_Set_At_48MHz, #1 542 | ENDM 543 | Set_LED_0 MACRO 544 | ENDM 545 | Clear_LED_0 MACRO 546 | ENDM 547 | Set_LED_1 MACRO 548 | ENDM 549 | Clear_LED_1 MACRO 550 | ENDM 551 | Set_LED_2 MACRO 552 | ENDM 553 | Clear_LED_2 MACRO 554 | ENDM 555 | Set_LED_3 MACRO 556 | ENDM 557 | Clear_LED_3 MACRO 558 | ENDM 559 | --------------------------------------------------------------------------------