├── .clang-format ├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── docs ├── Doxyfile ├── images │ ├── rc_pilot modules_transparent_bg.png │ └── rc_pilot modules_white_bg.png └── src │ ├── DoxygenLayout.xml │ ├── customdoxygen.css │ ├── footer.html │ ├── header.html │ └── mainpage.dox ├── include ├── feedback.h ├── flight_mode.h ├── input_manager.h ├── log_manager.h ├── mavlink_manager.h ├── mix.h ├── printf_manager.h ├── rc_pilot_defs.h ├── setpoint_manager.h ├── settings.h ├── state_estimator.h ├── thread_defs.h └── thrust_map.h ├── scripts ├── copy_logs └── transfer_rcpilot ├── settings ├── hex_6dof_settings.json └── pgaskell_settings.json └── src ├── feedback.c ├── input_manager.c ├── log_manager.c ├── main.c ├── mavlink_manager.c ├── mix.c ├── printf_manager.c ├── setpoint_manager.c ├── settings.c ├── state_estimator.c └── thrust_map.c /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | BreakBeforeBraces: Allman 4 | ColumnLimit: '100' 5 | Cpp11BracedListStyle: 'true' 6 | IndentWidth: '4' 7 | Language: Cpp 8 | NamespaceIndentation: None 9 | ReflowComments: 'true' 10 | SortIncludes: 'true' 11 | SpaceAfterCStyleCast: 'false' 12 | SpaceBeforeAssignmentOperators: 'true' 13 | SpaceBeforeParens: ControlStatements 14 | SpaceInEmptyParentheses: 'false' 15 | SpacesInAngles: 'false' 16 | SpacesInCStyleCastParentheses: 'false' 17 | SpacesInContainerLiterals: 'false' 18 | SpacesInParentheses: 'false' 19 | SpacesInSquareBrackets: 'false' 20 | Standard: Cpp11 21 | TabWidth: '4' 22 | UseTab: Never 23 | AlignAfterOpenBracket: DontAlign 24 | AccessModifierOffset: -4 25 | PointerAlignment: Left 26 | AllowShortFunctionsOnASingleLine: None 27 | ... 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | docs/html/* -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 StrawsonDesign 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # This is a general use makefile for robotics cape projects written in C. 2 | # Just change the target name to match your main source code filename. 3 | 4 | SRCDIR := src 5 | BINDIR := bin 6 | BUILDDIR := build 7 | INCLUDEDIR := include 8 | TARGET := $(BINDIR)/rc_pilot 9 | 10 | # file definitions for rules 11 | SOURCES := $(shell find $(SRCDIR) -type f -name *.c) 12 | OBJECTS := $(SOURCES:$(SRCDIR)/%.c=$(BUILDDIR)/%.o) 13 | INCLUDES := $(shell find $(INCLUDEDIR) -name '*.h') 14 | 15 | CC := gcc 16 | LINKER := gcc 17 | WFLAGS := -Wall -Wextra 18 | CFLAGS := -I $(INCLUDEDIR) 19 | OPT_FLAGS := -O1 20 | LDFLAGS := -lm -lrt -pthread -lrobotcontrol -ljson-c 21 | 22 | RM := rm -rf 23 | INSTALL := install -m 4755 24 | INSTALLDIR := install -d -m 755 25 | 26 | LINK := ln -s -f 27 | LINKDIR := /etc/robotcontrol 28 | LINKNAME := link_to_startup_program 29 | 30 | prefix ?= /usr 31 | 32 | 33 | # linking Objects 34 | $(TARGET): $(OBJECTS) 35 | @mkdir -p $(BINDIR) 36 | @$(LINKER) -o $(@) $(OBJECTS) $(LDFLAGS) 37 | @echo "made: $(@)" 38 | 39 | # rule for all other objects 40 | $(BUILDDIR)/%.o : $(SRCDIR)/%.c $(INCLUDES) 41 | @mkdir -p $(dir $(@)) 42 | @$(CC) -c $(CFLAGS) $(OPT_FLAGS) $(DEBUGFLAG) $< -o $(@) 43 | @echo "made: $(@)" 44 | 45 | all: $(TARGET) 46 | 47 | debug: 48 | $(MAKE) $(MAKEFILE) DEBUGFLAG="-g -D DEBUG" 49 | @echo "$(TARGET) Make Debug Complete" 50 | 51 | install: 52 | @$(INSTALLDIR) $(DESTDIR)$(prefix)/bin 53 | @$(INSTALL) $(TARGET) $(DESTDIR)$(prefix)/bin 54 | @echo "$(TARGET) Install Complete" 55 | 56 | clean: 57 | @$(RM) $(BINDIR) 58 | @$(RM) $(BUILDDIR) 59 | @$(RM) docs/html 60 | @echo "Library Clean Complete" 61 | 62 | uninstall: 63 | @$(RM) $(DESTDIR)$(prefix)/$(TARGET) 64 | @echo "$(TARGET) Uninstall Complete" 65 | 66 | runonboot: 67 | @$(MAKE) install 68 | @$(LINK) $(DESTDIR)$(prefix)/bin/$(TARGET) $(LINKDIR)/$(LINKNAME) 69 | @echo "$(TARGET) Set to Run on Boot" 70 | 71 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Multirotor Flight Controller for the Robotics Cape 2 | 3 | Please don't use this and expect it to work the way you want it to!!!!! 4 | I wrote this purely for my own use. NOT FOR PUBLIC USE!!!! 5 | It comes with no warranty, no manual, and I won't answer questions about 6 | how it works. If it breaks something, you get to keep both pieces. 7 | 8 | However, enough people have asked me for it so here it is. 9 | 10 | 11 | Note: depends on libjson-c-dev & libjson-c3 12 | sudo apt install libjson-c-dev libjson-c3 13 | 14 | also libroboticscape >v0.4.0 15 | -------------------------------------------------------------------------------- /docs/images/rc_pilot modules_transparent_bg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StrawsonDesign/rc_pilot/075c33f9fb397476872d076032bb4f2e56fcc579/docs/images/rc_pilot modules_transparent_bg.png -------------------------------------------------------------------------------- /docs/images/rc_pilot modules_white_bg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StrawsonDesign/rc_pilot/075c33f9fb397476872d076032bb4f2e56fcc579/docs/images/rc_pilot modules_white_bg.png -------------------------------------------------------------------------------- /docs/src/DoxygenLayout.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | -------------------------------------------------------------------------------- /docs/src/customdoxygen.css: -------------------------------------------------------------------------------- 1 | /* The standard CSS for doxygen 1.8.4 */ 2 | @import url(http://fonts.googleapis.com/css?family=Source+Sans+Pro:400,200,200italic,400italic); 3 | @import url(http://fonts.googleapis.com/css?family=Roboto:400,300,300italic,400italic,500,500italic,700,700italic,900,900italic&subset=latin,greek-ext,greek,latin-ext); 4 | body{ 5 | background-color: #fff; 6 | margin: 0; 7 | font-size: 12pt; 8 | } 9 | body, table, div, p, dl, 10 | table.directory, #nav-tree,#nav-tree .label { 11 | font-family: "Segoe UI", "Lucida Sans Unicode", Helvetica, Arial, Verdana, 'Roboto', sans-serif; 12 | color: #111; 13 | font-size: 1rem; 14 | } 15 | .arrow, 16 | .navpath li.navelem a, 17 | #navrow4 a,.icon, span.mlabel, 18 | tt, code, kbd, samp, a.el, 19 | pre.fragment, div.line, 20 | .mdescLeft, .mdescRight, 21 | .memItemLeft, .memItemRight, 22 | .memTemplItemLeft, .memTemplItemRight, .memTemplParams, 23 | .memname, 24 | .params .paramdir { 25 | font-family: Consolas, 'Courier New', courier, Courier, monospace; 26 | } 27 | .title, 28 | .fieldtable th{ 29 | font-family: "Cambria", serif; 30 | } 31 | #projectname,#projectbrief, 32 | h1, h2, h3 { 33 | font-family: "Segoe UI Light", 'Source Sans Pro', sans-serif; 34 | margin: 1.75em 0 .2em 0; 35 | font-weight: 200; 36 | } 37 | #projectname{color:#00afff;font-weight:350;} 38 | #projectbrief{color:#16499a;} 39 | h1,h2,h3{color: #000;} 40 | /* @group Heading Levels */ 41 | .title { 42 | color: #333; 43 | letter-spacing: 0.00em; 44 | font-size: 2rem; 45 | line-height: 2.1rem; 46 | font-weight: 500; 47 | margin: 10px 2px; 48 | } 49 | h1, h1.groupheader { 50 | font-weight: 200; 51 | color: #000000; 52 | letter-spacing: 0.00em; 53 | font-size: 2.5rem; 54 | line-height: 3.7rem; 55 | } 56 | h2, h2.groupheader { 57 | font-weight: 200; 58 | color: #000000; 59 | letter-spacing: 0.00em; 60 | font-size: 2.0rem; 61 | line-height: 2.3rem; 62 | letter-spacing: 0.01em; 63 | border-bottom: none; 64 | margin: 1.75em 0 .2em 0; 65 | padding: 0px; 66 | width: 100%; 67 | } 68 | h3.groupheader { 69 | font-weight: 200; 70 | font-size: 2.0rem; 71 | line-height: 2.1rem; 72 | color: #000; 73 | } 74 | h1, h2, h3, h4, h5, h6 { 75 | /*-webkit-transition: text-shadow:none;// 0.5s linear; 76 | -moz-transition: text-shadow:none;// 0.5s linear; 77 | -ms-transition: text-shadow:none;// 0.5s linear; 78 | -o-transition: text-shadow:none;// 0.5s linear; 79 | transition: text-shadow:none;// 0.5s linear; 80 | //margin-right: 15px; 81 | //margin: 42px 0px 20px 0px;*/ 82 | } 83 | h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow { 84 | text-shadow:none;//: 0 0 15px cyan; 85 | } 86 | dt { 87 | font-weight: bold; 88 | } 89 | div.multicol { 90 | -moz-column-gap: 1em; 91 | -webkit-column-gap: 1em; 92 | -moz-column-count: 3; 93 | -webkit-column-count: 3; 94 | } 95 | p.startli, p.startdd, p.starttd { 96 | margin-top: 2px; 97 | } 98 | p.endli { 99 | margin-bottom: 0px; 100 | } 101 | p.enddd { 102 | margin-bottom: 4px; 103 | } 104 | p.endtd { 105 | margin-bottom: 2px; 106 | } 107 | /* @end */ 108 | caption { 109 | font-weight: bold; 110 | } 111 | span.legend { 112 | font-size: 70%; 113 | text-align: center; 114 | } 115 | h3.version { 116 | font-size: 90%; 117 | text-align: center; 118 | } 119 | div.qindex, div.navtab{ 120 | background-color: #00aff0;/*blue*/ 121 | border: 1px solid #00aff0;/*blue*/ 122 | text-align: center; 123 | } 124 | div.qindex, div.navpath { 125 | width: 100%; 126 | line-height: 140%; 127 | } 128 | div.navtab { 129 | margin-right: 15px; 130 | } 131 | /* @group Link Styling */ 132 | a { 133 | color: #16499a;/*darkBlue*/ 134 | font-weight: normal; 135 | text-decoration: none; 136 | } 137 | a:hover { 138 | text-decoration: underline; 139 | } 140 | .contents a, .contents a:visited { 141 | color: #16499a;/*darkBlue*/ 142 | } 143 | a.qindex, a.qindex:visited { 144 | color: #fff; 145 | font-weight: normal; 146 | } 147 | a.qindexHL { 148 | font-weight: normal; 149 | background-color: #9CAFD4; 150 | color: #ffffff; 151 | border: 1px double #869DCA; 152 | } 153 | .contents a.qindexHL:visited { 154 | color: #ffffff; 155 | } 156 | a.el{ 157 | font-weight: normal; 158 | } 159 | a.elRef { 160 | } 161 | a.code, a.code:visited { 162 | color: #16499a;/*darkBlue*/ 163 | } 164 | a.codeRef, a.codeRef:visited { 165 | color: #16499a;/*darkBlue*/ 166 | } 167 | /* @end */ 168 | dl.el { 169 | margin-left: -1cm; 170 | } 171 | pre.fragment { 172 | border: 1px solid #C4CFE5; 173 | background-color: #FBFCFD; 174 | padding: 4px 6px; 175 | margin: 4px 8px 4px 2px; 176 | overflow: auto; 177 | word-wrap: break-word; 178 | font-size: 10pt; 179 | line-height: 125%; 180 | } 181 | div.fragment { 182 | padding: 6px 10px; 183 | margin: 15px 0px; 184 | border: solid 1px rgb(221, 221, 221); 185 | border-radius: 3px; 186 | background-color: rgb(248, 248, 248); 187 | } 188 | div.line { 189 | min-height: 11pt; 190 | line-height: 1.0; 191 | text-wrap: unrestricted; 192 | white-space: -moz-pre-wrap; /* Moz */ 193 | white-space: -pre-wrap; /* Opera 4-6 */ 194 | white-space: -o-pre-wrap; /* Opera 7 */ 195 | white-space: pre-wrap; /* CSS3 */ 196 | word-wrap: break-word; /* IE 5.5+ */ 197 | text-indent: -53px; 198 | padding-left: 53px; 199 | padding-bottom: 0px; 200 | margin: 0px; 201 | -webkit-transition-property: background-color, box-shadow; 202 | -webkit-transition-duration: 0.5s; 203 | -moz-transition-property: background-color, box-shadow; 204 | -moz-transition-duration: 0.5s; 205 | -ms-transition-property: background-color, box-shadow; 206 | -ms-transition-duration: 0.5s; 207 | -o-transition-property: background-color, box-shadow; 208 | -o-transition-duration: 0.5s; 209 | transition-property: background-color, box-shadow; 210 | transition-duration: 0.5s; 211 | } 212 | div.line.glow { 213 | background-color: blue; 214 | box-shadow: 0 0 10px blue; 215 | } 216 | span.lineno { 217 | padding-right: 4px; 218 | text-align: right; 219 | border-right: 2px solid #0F0; 220 | background-color: #E8E8E8; 221 | white-space: pre; 222 | } 223 | span.lineno a { 224 | background-color: #D8D8D8; 225 | } 226 | span.lineno a:hover { 227 | background-color: #C8C8C8; 228 | } 229 | div.ah { 230 | background-color: #16499a;/*darkBlue*/ 231 | font-weight: lighter; 232 | color: #fff;/*white*/ 233 | margin-bottom: 3px; 234 | margin-top: 3px; 235 | padding: 0.2em; 236 | border: none; 237 | border-radius: 0.0em; 238 | -webkit-border-radius: .0em; 239 | -moz-border-radius: .0em; 240 | box-shadow: none; 241 | -webkit-box-shadow: none; 242 | -moz-box-shadow: none; 243 | background-image: none; 244 | } 245 | div.groupHeader { 246 | margin-left: 0px; 247 | margin-top: 9px; 248 | margin-bottom: 4.7px; 249 | font-size: 19px; 250 | font-weight: normal; 251 | } 252 | div.groupText { 253 | margin-left: 16px; 254 | font-style: italic; 255 | } 256 | div.contents { 257 | margin-top: 10px; 258 | margin-left: 12px; 259 | margin-right: 8px; 260 | } 261 | td.indexkey { 262 | background-color: #EBEFF6; 263 | font-weight: bold; 264 | border: 1px solid #C4CFE5; 265 | margin: 2px 0px 2px 0; 266 | padding: 2px 10px; 267 | white-space: nowrap; 268 | vertical-align: top; 269 | } 270 | td.indexvalue { 271 | background-color: #EBEFF6; 272 | border: 1px solid #C4CFE5; 273 | padding: 2px 10px; 274 | margin: 2px 0px; 275 | } 276 | tr.memlist { 277 | background-color: #EEF1F7; 278 | } 279 | p.formulaDsp { 280 | text-align: center; 281 | } 282 | img.formulaDsp { 283 | } 284 | img.formulaInl { 285 | vertical-align: middle; 286 | } 287 | div.center { 288 | text-align: center; 289 | margin-top: 0px; 290 | margin-bottom: 0px; 291 | padding: 0px; 292 | } 293 | div.center img { 294 | border: 0px; 295 | } 296 | address.footer { 297 | text-align: right; 298 | padding-right: 12px; 299 | } 300 | img.footer { 301 | border: 0px; 302 | vertical-align: middle; 303 | width:3.5rem; 304 | } 305 | /* @group Code colorization */ 306 | span.keyword { 307 | color: #008000 308 | } 309 | span.keywordtype { 310 | color: #604020 311 | } 312 | span.keywordflow { 313 | color: #e08000 314 | } 315 | span.comment { 316 | color: #800000 317 | } 318 | span.preprocessor { 319 | color: #806020 320 | } 321 | span.stringliteral { 322 | color: #002080 323 | } 324 | span.charliteral { 325 | color: #008080 326 | } 327 | span.vhdldigit { 328 | color: #ff00ff 329 | } 330 | span.vhdlchar { 331 | color: #000000 332 | } 333 | span.vhdlkeyword { 334 | color: #700070 335 | } 336 | span.vhdllogic { 337 | color: #ff0000 338 | } 339 | blockquote { 340 | background-color: #F7F8FB; 341 | border-left: 2px solid #9CAFD4; 342 | margin: 0 24px 0 4px; 343 | padding: 0 12px 0 16px; 344 | } 345 | /* @end */ 346 | /* 347 | .search { 348 | color: #003399; 349 | font-weight: bold; 350 | } 351 | form.search { 352 | margin-bottom: 0px; 353 | margin-top: 0px; 354 | } 355 | input.search { 356 | font-size: 75%; 357 | color: #000080; 358 | font-weight: normal; 359 | background-color: #e8eef2; 360 | } 361 | */ 362 | td.tiny { 363 | font-size: 75%; 364 | } 365 | .dirtab { 366 | padding: 4px; 367 | border-collapse: collapse; 368 | border: 1px solid #A3B4D7; 369 | } 370 | th.dirtab { 371 | background: #EBEFF6; 372 | font-weight: bold; 373 | } 374 | hr { 375 | height: 0px; 376 | border: none; 377 | border-top: 1px solid #aaa; 378 | } 379 | hr.footer { 380 | height: 0px; 381 | border-top: 3px solid #aaa; 382 | } 383 | /* @group Member Descriptions */ 384 | table.memberdecls { 385 | border-spacing: 0px; 386 | padding: 0px; 387 | } 388 | .memberdecls td, .fieldtable tr { 389 | -webkit-transition-property: background-color, box-shadow; 390 | -webkit-transition-duration: 0.5s; 391 | -moz-transition-property: background-color, box-shadow; 392 | -moz-transition-duration: 0.5s; 393 | -ms-transition-property: background-color, box-shadow; 394 | -ms-transition-duration: 0.5s; 395 | -o-transition-property: background-color, box-shadow; 396 | -o-transition-duration: 0.5s; 397 | transition-property: background-color, box-shadow; 398 | transition-duration: 0.5s; 399 | } 400 | .memberdecls td.glow, .fieldtable tr.glow { 401 | background-color: blue; 402 | box-shadow: 0 0 15px blue; 403 | } 404 | .mdescLeft, .mdescRight, 405 | .memItemLeft, .memItemRight, 406 | .memTemplItemLeft, .memTemplItemRight, .memTemplParams { 407 | background-color: #f0f4f8; 408 | border: none; 409 | margin: 4px; 410 | padding: 1px 0 0 8px; 411 | } 412 | .mdescLeft, .mdescRight { 413 | padding: 0px 8px 4px 8px; 414 | color: #555; 415 | font-style: italic; 416 | font-size: .9rem; 417 | } 418 | .memSeparator { 419 | border-bottom: 1px solid #fff; 420 | line-height: 1px; 421 | margin: 0px; 422 | padding: 0px; 423 | } 424 | .memItemLeft, .memTemplItemLeft { 425 | white-space: nowrap; 426 | } 427 | .memItemLeft{padding-top:6px;} 428 | .memItemRight, .memTemplItemRight { 429 | padding-top:6px; 430 | width: 100%; 431 | } 432 | .memTemplParams { 433 | color: #4665A2; 434 | white-space: nowrap; 435 | font-size: 80%; 436 | } 437 | /* @end */ 438 | /* @group Member Details */ 439 | /* Styles for detailed member documentation */ 440 | .memtemplate { 441 | font-size: .8rem; 442 | color: #666; 443 | font-weight: normal; 444 | margin-left: 2px; 445 | } 446 | .memnav { 447 | background-color: #EBEFF6; 448 | border: 1px solid #A3B4D7; 449 | text-align: center; 450 | margin: 2px; 451 | margin-right: 15px; 452 | padding: 2px; 453 | } 454 | .mempage { 455 | width: 100%; 456 | } 457 | .memitem { 458 | border: 1px solid #ccc; 459 | padding: 0; 460 | margin-bottom: 10px; 461 | margin-right: 5px; 462 | -webkit-transition: box-shadow 0.5s linear; 463 | -moz-transition: box-shadow 0.5s linear; 464 | -ms-transition: box-shadow 0.5s linear; 465 | -o-transition: box-shadow 0.5s linear; 466 | transition: box-shadow 0.5s linear; 467 | display: table !important; 468 | width: 100%; 469 | } 470 | .memitem.glow { 471 | box-shadow: 0 0 15px blue; 472 | } 473 | .memname { 474 | font-weight: normal; 475 | margin-left: 0px; 476 | } 477 | .memname a.el{ 478 | font-weight: normal; 479 | } 480 | .memname td { 481 | vertical-align: bottom; 482 | } 483 | .memproto, dl.reflist dt { 484 | margin-top: 1.5em; 485 | border: none; 486 | border-radius: 0; 487 | padding: 6px; 488 | color: #00aff0; 489 | font-weight: normal; 490 | text-shadow:none; 491 | background-image:none; 492 | background-color: #f0f4f8; 493 | /* opera specific markup */ 494 | box-shadow: none; 495 | /* firefox specific markup */ 496 | } 497 | .memdoc, dl.reflist dd { 498 | border: none; 499 | padding: 6px; 500 | border-top-width: 0; 501 | background-image: none; 502 | background-color: #FFFFFF; 503 | /* opera specific markup */ 504 | border-bottom-left-radius: 0px; 505 | border-bottom-right-radius: 0px; 506 | box-shadow: none; 507 | /* firefox specific markup */ 508 | -moz-border-radius-bottomleft: 0px; 509 | -moz-border-radius-bottomright: 0px; 510 | -moz-box-shadow: none; 511 | /* webkit specific markup */ 512 | -webkit-border-bottom-left-radius: 0px; 513 | -webkit-border-bottom-right-radius: 0px; 514 | -webkit-box-shadow: none; 515 | } 516 | dl.reflist dt { 517 | padding: 5px; 518 | } 519 | dl.reflist dd { 520 | margin: 0px 0px 10px 0px; 521 | padding: 5px; 522 | } 523 | .paramkey { 524 | text-align: right; 525 | } 526 | .paramtype { 527 | white-space: nowrap; 528 | } 529 | .paramname { 530 | color: #602020; 531 | white-space: nowrap; 532 | } 533 | .paramname em { 534 | font-style: italic; 535 | font-weight: normal; 536 | } 537 | .paramname code { 538 | line-height: 14px; 539 | } 540 | .params, .retval, .exception, .tparams { 541 | margin-left: 0px; 542 | padding-left: 0px; 543 | } 544 | .params .paramname, .retval .paramname { 545 | font-family: Consolas, "Courier New", courier, Courier, monospace; 546 | font-size:105%; 547 | font-style: italic; 548 | font-weight: normal; 549 | text-shadow:none; 550 | background-color: #f8f8f8; 551 | display: inline; 552 | margin: 0 16px 0 0; 553 | } 554 | .params .paramtype { 555 | font-style: italic; 556 | vertical-align: top; 557 | } 558 | .params .paramdir { 559 | vertical-align: top; 560 | } 561 | table.mlabels { 562 | border-spacing: 0px; 563 | } 564 | td.mlabels-left { 565 | width: 100%; 566 | padding: 0px; 567 | } 568 | td.mlabels-right { 569 | vertical-align: middle; 570 | padding: 0px; 571 | white-space: nowrap; 572 | } 573 | span.mlabels { 574 | margin-left: 8px; 575 | } 576 | span.mlabel { 577 | background-color:#f8f8f8; /*grey*/ 578 | border:1px solid #ccc; 579 | text-shadow:none; 580 | color: #00aff0;/*blue*/ 581 | margin-right: 4px; 582 | padding: 2px 3px; 583 | border-radius: 0px; 584 | font-size: .7rem; 585 | white-space: nowrap; 586 | vertical-align: middle; 587 | } 588 | 589 | /* @end */ 590 | /* these are for tree view when not used as main index */ 591 | div.directory { 592 | margin: 10px 0px; 593 | border-top: 1px solid #A8B8D9; 594 | border-bottom: 1px solid #A8B8D9; 595 | width: 100%; 596 | } 597 | .directory table { 598 | border-collapse:collapse; 599 | } 600 | .directory td { 601 | margin: 0px; 602 | padding: 0px; 603 | vertical-align: top; 604 | } 605 | .directory td.entry { 606 | white-space: nowrap; 607 | padding-right: 6px; 608 | padding-top: 3px; 609 | } 610 | .directory td.entry a { 611 | outline:none; 612 | } 613 | .directory td.entry a img { 614 | border: none; 615 | } 616 | .directory td.desc { 617 | width: 100%; 618 | padding-left: 6px; 619 | padding-right: 6px; 620 | padding-top: 3px; 621 | border-left: 1px solid rgba(0,0,0,0.05); 622 | } 623 | .directory tr.even { 624 | padding-left: 6px; 625 | background-color: #F7F8FB; 626 | } 627 | .directory img { 628 | vertical-align: -30%; 629 | } 630 | .directory .levels { 631 | white-space: nowrap; 632 | width: 100%; 633 | text-align: right; 634 | font-size: 9pt; 635 | } 636 | .directory .levels span { 637 | cursor: pointer; 638 | padding-left: 2px; 639 | padding-right: 2px; 640 | color: #3D578C; 641 | } 642 | div.dynheader { 643 | margin-top: 8px; 644 | -webkit-touch-callout: none; 645 | -webkit-user-select: none; 646 | -khtml-user-select: none; 647 | -moz-user-select: none; 648 | -ms-user-select: none; 649 | user-select: none; 650 | } 651 | address { 652 | font-style: normal; 653 | color: #2A3D61; 654 | } 655 | table.doxtable { 656 | border-collapse:collapse; 657 | margin-top: 4px; 658 | margin-bottom: 4px; 659 | } 660 | table.doxtable td, table.doxtable th { 661 | border: 1px solid #2D4068; 662 | padding: 3px 7px 2px; 663 | } 664 | table.doxtable th { 665 | background-color: #374F7F; 666 | color: #FFFFFF; 667 | font-size: 110%; 668 | padding-bottom: 4px; 669 | padding-top: 5px; 670 | } 671 | table.fieldtable { 672 | /*width: 100%;*/ 673 | margin-bottom: 10px; 674 | border: 1px solid #eee; 675 | border-spacing: 0px; 676 | -moz-border-radius: 0px; 677 | -webkit-border-radius: 0px; 678 | border-radius: 0px; 679 | -moz-box-shadow: none; 680 | -webkit-box-shadow: none; 681 | box-shadow: none; 682 | } 683 | .fieldtable td, .fieldtable th { 684 | padding: 3px 7px 2px; 685 | } 686 | .fieldtable td.fieldtype, .fieldtable td.fieldname { 687 | white-space: nowrap; 688 | border-right: 1px solid #ccc; 689 | border-bottom: 1px solid #ccc; 690 | vertical-align: top; 691 | } 692 | .fieldtable td.fieldname { 693 | padding-top: 3px; 694 | } 695 | .fieldtable td.fielddoc { 696 | border-bottom: 1px solid #ccc; 697 | /*width: 100%;*/ 698 | } 699 | .fieldtable td.fielddoc p:first-child { 700 | margin-top: 0px; 701 | } 702 | .fieldtable td.fielddoc p:last-child { 703 | margin-bottom: 2px; 704 | } 705 | .fieldtable tr:last-child td { 706 | border-bottom: none; 707 | } 708 | .fieldtable th { 709 | background-image:none; 710 | background-repeat:repeat-x; 711 | background-color:#eee; 712 | font-size: 1.05rem; 713 | font-weight:normal; 714 | color: #00aff0; 715 | padding-bottom: 4px; 716 | padding-top: 5px; 717 | text-align:left; 718 | -moz-border-radius:0; 719 | -webkit-border-radius:0px; 720 | border-radius: 0px; 721 | border-bottom: 1px solid #eee; 722 | } 723 | .tabsearch { 724 | top: 0px; 725 | left: 10px; 726 | height: 36px; 727 | background-image: url('tab_b.png'); 728 | z-index: 101; 729 | overflow: hidden; 730 | font-size: 13px; 731 | } 732 | .navpath ul 733 | { 734 | font-size: 11px; 735 | background-image:url('tab_b.png'); 736 | background-repeat:repeat-x; 737 | background-position: 0 -5px; 738 | height:30px; 739 | line-height:30px; 740 | color:#8AA0CC; 741 | border:solid 1px #C2CDE4; 742 | overflow:hidden; 743 | margin:0px; 744 | padding:0px; 745 | } 746 | .navpath li 747 | { 748 | list-style-type:none; 749 | float:left; 750 | padding-left:10px; 751 | padding-right:15px; 752 | background-image:url('bc_s.png'); 753 | background-repeat:no-repeat; 754 | background-position:right; 755 | color:#fff; 756 | } 757 | .navpath li.navelem a 758 | { 759 | height:32px; 760 | display:block; 761 | text-decoration: none; 762 | outline: none; 763 | color: #fff; 764 | font-size:.8rem; 765 | text-shadow:none; 766 | text-decoration: none; 767 | } 768 | .navpath li.navelem a:hover 769 | { 770 | color:#6884BD; 771 | } 772 | .navpath li.footer 773 | { 774 | list-style-type:none; 775 | float:right; 776 | padding-left:10px; 777 | padding-right:15px; 778 | background-image:none; 779 | background-repeat:no-repeat; 780 | background-position:right; 781 | color:#ccc; 782 | font-size: 8pt; 783 | } 784 | div.summary 785 | { 786 | float: right; 787 | font-size: 8pt; 788 | padding-right: 5px; 789 | width: 50%; 790 | text-align: right; 791 | } 792 | div.summary a 793 | { 794 | white-space: nowrap; 795 | } 796 | div.ingroups 797 | { 798 | font-size: 8pt; 799 | width: 50%; 800 | text-align: left; 801 | } 802 | div.ingroups a 803 | { 804 | white-space: nowrap; 805 | } 806 | div.header 807 | { 808 | background-image: none; 809 | background-color: #f8f8f8; 810 | margin: 0px; 811 | border: none; 812 | } 813 | div.headertitle 814 | { 815 | padding: 5px 5px 5px 10px; 816 | } 817 | dl 818 | { 819 | padding: 0 0 0 10px; 820 | } 821 | /* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug */ 822 | dl.section 823 | { 824 | margin-left: 0px; 825 | padding-left: 0px; 826 | } 827 | dl.note 828 | { 829 | margin-left: 0px; 830 | padding: 6px 0px 3px 8px; 831 | border-left: 6px solid; 832 | border-color: #D0C000; 833 | background-color: #fff799 834 | } 835 | dl.warning, dl.attention 836 | { 837 | margin-left: 0px; 838 | padding: 6px 0px 3px 8px; 839 | border-left: 6px solid; 840 | border-color: #FF0000; 841 | } 842 | dl.pre, dl.post, dl.invariant 843 | { 844 | margin-left:-7px; 845 | padding-left: 3px; 846 | border-left:4px solid; 847 | border-color: #00D000; 848 | } 849 | dl.deprecated 850 | { 851 | margin-left: 0px; 852 | padding: 6px 0px 3px 8px; 853 | border-left: 6px solid; 854 | border-color: #505050; 855 | } 856 | dl.deprecated dt a.el 857 | { 858 | font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; 859 | } 860 | dl.todo 861 | { 862 | margin-left: 0px; 863 | padding: 6px 0px 3px 8px; 864 | border-left:4px solid; 865 | border-color: #00C0E0; 866 | } 867 | dl.test 868 | { 869 | margin-left:-7px; 870 | padding-left: 3px; 871 | border-left:4px solid; 872 | border-color: #3030E0; 873 | } 874 | dl.bug 875 | { 876 | margin-left:-7px; 877 | padding-left: 3px; 878 | border-left:4px solid; 879 | border-color: #C08050; 880 | } 881 | dl.section dd { 882 | margin-bottom: 6px; 883 | } 884 | #projectlogo 885 | { 886 | text-align: center; 887 | vertical-align: bottom; 888 | border-collapse: separate; 889 | } 890 | #projectlogo img 891 | { 892 | border: 0px none; 893 | width: 4.5rem; 894 | float:left; 895 | } 896 | #projectname 897 | { 898 | font-size: 3rem; 899 | margin: 0px; 900 | padding: 2px 0px; 901 | float:left; 902 | } 903 | #projectbrief 904 | { 905 | font-size: 1.2rem; 906 | margin: 0px; 907 | padding: 2.5rem 0 0 .25rem; 908 | float:left; 909 | font-style: italic; 910 | } 911 | #projectnumber 912 | { 913 | font: 50% Tahoma, Arial,sans-serif; 914 | margin: 0px; 915 | padding: 0px; 916 | } 917 | #titlearea 918 | { 919 | padding: 0px; 920 | margin: 0px; 921 | width: 100%; 922 | border-bottom: 1px solid #5373B4; 923 | backround-color: gainsboro; 924 | } 925 | .image 926 | { 927 | text-align: center; 928 | } 929 | .dotgraph 930 | { 931 | text-align: center; 932 | } 933 | .mscgraph 934 | { 935 | text-align: center; 936 | } 937 | .caption 938 | { 939 | font-weight: bold; 940 | } 941 | div.zoom 942 | { 943 | border: 1px solid #90A5CE; 944 | } 945 | dl.citelist { 946 | margin-bottom:50px; 947 | } 948 | dl.citelist dt { 949 | color:#334975; 950 | float:left; 951 | font-weight:bold; 952 | margin-right:10px; 953 | padding:5px; 954 | } 955 | dl.citelist dd { 956 | margin:2px 0; 957 | padding:5px 0; 958 | } 959 | div.toc { 960 | padding: 14px 25px; 961 | background-color: #F4F6FA; 962 | border: 1px solid #D8DFEE; 963 | border-radius: 7px 7px 7px 7px; 964 | float: right; 965 | height: auto; 966 | margin: 0 20px 10px 10px; 967 | width: 200px; 968 | } 969 | div.toc li { 970 | background: url("bdwn.png") no-repeat scroll 0 5px transparent; 971 | font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif; 972 | margin-top: 5px; 973 | padding-left: 10px; 974 | padding-top: 2px; 975 | } 976 | div.toc h3 { 977 | font: bold 12px/1.2 Arial,FreeSans,sans-serif; 978 | color: #4665A2; 979 | border-bottom: 0 none; 980 | margin: 0; 981 | } 982 | div.toc ul { 983 | list-style: none outside none; 984 | border: medium none; 985 | padding: 0px; 986 | } 987 | div.toc li.level1 { 988 | margin-left: 0px; 989 | } 990 | div.toc li.level2 { 991 | margin-left: 15px; 992 | } 993 | div.toc li.level3 { 994 | margin-left: 30px; 995 | } 996 | div.toc li.level4 { 997 | margin-left: 45px; 998 | } 999 | .inherit_header { 1000 | font-weight: bold; 1001 | color: gray; 1002 | cursor: pointer; 1003 | -webkit-touch-callout: none; 1004 | -webkit-user-select: none; 1005 | -khtml-user-select: none; 1006 | -moz-user-select: none; 1007 | -ms-user-select: none; 1008 | user-select: none; 1009 | } 1010 | .inherit_header td { 1011 | padding: 6px 0px 2px 5px; 1012 | } 1013 | .inherit { 1014 | display: none; 1015 | } 1016 | tr.heading h2 { 1017 | margin-top: 42px; 1018 | margin-bottom: 20px; 1019 | } 1020 | @media print 1021 | { 1022 | #top { display: none; } 1023 | #side-nav { display: none; } 1024 | #nav-path { display: none; } 1025 | body { overflow:visible; } 1026 | h1, h2, h3, h4, h5, h6 { page-break-after: avoid; } 1027 | .summary { display: none; } 1028 | .memitem { page-break-inside: avoid; } 1029 | #doc-content 1030 | { 1031 | margin-left:0 !important; 1032 | height:auto !important; 1033 | width:auto !important; 1034 | overflow:inherit; 1035 | display:inline; 1036 | } 1037 | } 1038 | .tabs, .tabs2, .tabs3 { 1039 | background-image: none; 1040 | background-color: #16499a; /*darkBlue*/ 1041 | color: white; 1042 | } 1043 | .tablist li.current a{ 1044 | text-shadow: none; 1045 | } 1046 | .tabs2 { 1047 | background-color: #00aff0; /*blue*/ 1048 | font-size:11pt; 1049 | } 1050 | #navrow3{background-color: #ddd;} 1051 | #navrow3 a{color: #16499a;} 1052 | #navrow4{background-color:#eee; } 1053 | #navrow4 li{border: 1px solid #e8e8e8;} 1054 | #navrow4 a{color:#00aff0;font-weight: normal;} 1055 | .tablist li { 1056 | background-image: none; 1057 | font-size:0.8rem; 1058 | line-height:1.65rem; 1059 | } 1060 | .tablist a { 1061 | background-image: none; 1062 | color: white; 1063 | text-shadow:none; 1064 | } 1065 | .tablist a:hover { 1066 | background-image: none; 1067 | text-shadow:none; 1068 | } 1069 | .tablist li.current a { 1070 | background-image: none; 1071 | color: #fff; 1072 | text-shadow:none; 1073 | } 1074 | .tabs li.current { 1075 | background-color: #00aff0; /*blue*/ 1076 | color: #fff; 1077 | } 1078 | .tabs2 li.current { 1079 | background-color: lightskyblue;/* #f0a30a;amber*/ 1080 | } 1081 | .navpath { 1082 | border: none; 1083 | } 1084 | .navpath ul { 1085 | background-image:url('tab_a.png'); 1086 | background-repeat:repeat-x; 1087 | background-position: 0 -5px; 1088 | /* background-image: none;*/ 1089 | /* background-color: lightskyblue; /*lightblue*/*/ 1090 | border: none; 1091 | } 1092 | .navpath li { 1093 | background-image: none; 1094 | } 1095 | .navpath li.navelem a { 1096 | background-image: none; 1097 | color: white; 1098 | text-shadow:none;/*: none;*/ 1099 | } 1100 | .navpath li.navelem a:hover { 1101 | background-image: none; 1102 | color: white; 1103 | text-shadow:none;/*: none;*/ 1104 | } 1105 | .icona { 1106 | width: 1.5rem; 1107 | height: 1.45rem; 1108 | display: inline-block; 1109 | } 1110 | .icon { 1111 | font-weight: bold; 1112 | font-size: .8rem; 1113 | height: 1rem; 1114 | width: 1rem; 1115 | display: inline-block; 1116 | background-color: #87794e;/*taupe*/ 1117 | color: white; 1118 | text-align: center; 1119 | border-radius: 0px; 1120 | margin-left: 2px; 1121 | margin-right: 2px; 1122 | } 1123 | /* navtree */ 1124 | #nav-tree { 1125 | background-color: #fdfdfd; 1126 | background-image: none; 1127 | font-size: 1rem; 1128 | } 1129 | #nav-tree .selected { 1130 | background-image: none; 1131 | background-color:lightskyblue;/*#f0a30a;/amber*/ 1132 | text-shadow:none; 1133 | } 1134 | #nav-tree .label{ 1135 | font-size: .9rem; 1136 | } 1137 | .ui-resizable-e{ 1138 | background-image: none; 1139 | background-color: #c4c8ca; 1140 | } -------------------------------------------------------------------------------- /docs/src/footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | 13 | 14 |
15 | $generatedby   16 | doxygen 17 | $doxygenversion 18 |
19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /docs/src/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | $projectname: $title 10 | $title 11 | 12 | 13 | 14 | $treeview 15 | $search 16 | $mathjax 17 | 18 | $extrastylesheet 19 | 20 | 21 |
22 | 23 | 24 |
25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 38 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 |
33 |
$projectname 34 |  $projectnumber 35 |
36 |
$projectbrief
37 | 		42 |
$projectbrief
43 | 		$searchbox
54 |
55 | 56 | 57 | -------------------------------------------------------------------------------- /docs/src/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \mainpage 4 | 5 | 6 | This package contains the C library and example/testing programs for the Robot Control project. This project began as a hardware interface for the Robotics Cape and later the BeagleBone Blue and was originally called Robotics_Cape_Installer. It grew to include an extensive math library for discrete time feedback control, as well as a plethora of POSIX-compliant functions for timing, multithreading, program flow, and lots more. Everything is aimed at developing robotics software on embedded computers. 7 | 8 | This package ships with official BeagleBone images and is focused on, but not excludive to, the BeagleBoard platform. The library and example programs are primarily written in C, however there also exists the RCPY python interface to this package available at . 9 | 10 | The master branch is always the most current but not necessarily stable. See [releases](https://github.com/StrawsonDesign/Robotics_Cape_Installer/releases) page for older stable versions or install from BeagleBoard.org repositories. 11 | 12 | To get started, visit the [user manual](manual.html) 13 | 14 | 15 | */ 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /include/feedback.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feedback.h 3 | * 4 | * @brief Functions to run the feedback controller 5 | * 6 | * Here lies the heart and soul of the operation. feedback_init(void) pulls 7 | * in the control constants from settings module and sets up the discrete 8 | * controllers. From then on out, feedback_march(void) should be called by the 9 | * IMU interrupt at feedback_hz until the program is shut down. 10 | * feedback_march(void) will monitor the setpoint which is constantly being 11 | * changed by setpoint_manager(void). 12 | * 13 | */ 14 | 15 | #ifndef FEEDBACK_H 16 | #define FEEDBACK_H 17 | 18 | #include 19 | #include // for uint64_t 20 | 21 | /** 22 | * This is the state of the feedback loop. contains most recent values 23 | * reported by the feedback controller. Should only be written to by the 24 | * feedback controller after initialization. 25 | */ 26 | typedef struct feedback_state_t 27 | { 28 | int initialized; ///< set to 1 after feedback_init(void) 29 | arm_state_t arm_state; ///< actual arm state as reported by feedback controller 30 | uint64_t arm_time_ns; ///< time since boot when controller was armed 31 | uint64_t loop_index; ///< increases every time feedback loop runs 32 | uint64_t last_step_ns; ///< last time controller has finished a step 33 | 34 | double u[6]; ///< siso controller outputs 35 | double m[8]; ///< signals sent to motors after mapping 36 | } feedback_state_t; 37 | 38 | extern feedback_state_t fstate; 39 | 40 | /** 41 | * @brief Initial setup of all feedback controllers. Should only be called 42 | * once on program start. 43 | * 44 | * @param setpoint pointer to global setpoint struct 45 | * @param settings pointer to global settings struct 46 | * 47 | * @return 0 on success, -1 on failure 48 | */ 49 | int feedback_init(void); 50 | 51 | /** 52 | * @brief marches feedback controller forward one step 53 | * 54 | * This is called AFTER state_estimator_march and actually sends signals to the 55 | * motors. This can as is still safely called when Disarmed. 56 | * 57 | * @return 0 on success, -1 on failure 58 | */ 59 | int feedback_march(void); 60 | 61 | /** 62 | * @brief This is how outside functions should stop the flight controller. 63 | * 64 | * It would be reasonable to set motors to 0 here, but since this 65 | * function can be called from anywhere that might produce 66 | * conflicts. Instead the interrupt service routine will do this on 67 | * the next loop after disarming to maintain timing of pulses to the 68 | * motors 69 | * 70 | * @return 0 on success, -1 on failure 71 | */ 72 | int feedback_disarm(void); 73 | 74 | /** 75 | * @brief This is how outside functions should start the flight controller. 76 | * 77 | * @return 0 on success, -1 on failure 78 | */ 79 | int feedback_arm(void); 80 | 81 | /** 82 | * @brief Cleanup the feedback controller, freeing memory 83 | * 84 | * @return 0 on success, -1 on failure 85 | */ 86 | int feedback_cleanup(void); 87 | 88 | #endif // FEEDBACK_H 89 | -------------------------------------------------------------------------------- /include/flight_mode.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | */ 4 | 5 | #ifndef FLIGHT_MODE_H 6 | #define FLIGHT_MODE_H 7 | 8 | /** 9 | * This is how the user interacts with the setpoint manager. 10 | */ 11 | typedef enum flight_mode_t 12 | { 13 | /** 14 | * test_bench mode does no feedback at all, it takes the raw user inputs 15 | * and directly outputs to the motors. This could technically fly but 16 | * would not be easy! Designed for confirming mixing matrix and motors 17 | * are working. maps Z,Roll,Pitch,Yaw 18 | */ 19 | TEST_BENCH_4DOF, 20 | /** 21 | * test_bench mode does no feedback at all, it takes the raw user inputs 22 | * and directly outputs to the motors. This could technically fly but 23 | * would not be easy! Designed for confirming mixing matrix and motors 24 | * are working. maps X,Y,Z,Yaw 25 | */ 26 | TEST_BENCH_6DOF, 27 | /** 28 | * user inputs translate directly to the throttle, roll, pitch, & yaw 29 | * setpoints. No altitude feedback control. On 6DOF platforms the X & Y 30 | * thrust directions are left at 0. 31 | */ 32 | DIRECT_THROTTLE_4DOF, 33 | /** 34 | * user inputs translate directly to the throttle, roll, pitch, & yaw 35 | * setpoints. No altitude feedback control.Roll and pitch are left at 0 36 | */ 37 | DIRECT_THROTTLE_6DOF, 38 | /** 39 | * like DIRECT_THROTTLE for roll/pitch/yaw but feedback is performed to 40 | * hold altitude setpoint which is them moved up and down steadily based 41 | * on user input. 42 | */ 43 | ALT_HOLD_4DOF, 44 | /** 45 | * like DIRECT_THROTTLE for roll/pitch/yaw but feedback is performed to 46 | * hold altitude setpoint which is them moved up and down steadily based 47 | * on user input. 48 | */ 49 | ALT_HOLD_6DOF, 50 | /** 51 | * Control sticks translate to velocity setpoints in horizontal 52 | * translation X and Y. Yaw and Altitude are still position setpoints 53 | * like alt_hold 54 | */ 55 | VELOCITY_CONTROL_4DOF, 56 | /** 57 | * Control sticks translate to velocity setpoints in horizontal 58 | * translation X and Y. Yaw and Altitude are still position setpoints 59 | * like alt_hold 60 | */ 61 | VELOCITY_CONTROL_6DOF, 62 | /** 63 | * PG: TODO: What do you intend for this mode? 64 | */ 65 | POSITION_CONTROL_4DOF, 66 | /** 67 | * PG: TODO: What do you intend for this mode? 68 | */ 69 | POSITION_CONTROL_6DOF 70 | 71 | } flight_mode_t; 72 | 73 | #endif -------------------------------------------------------------------------------- /include/input_manager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * Functions to start and stop the input manager thread which is the translation 5 | * beween control inputs from DSM to the user_input struct which is read by the 6 | * setpoint manager. TODO: Allow other inputs such as mavlink 7 | */ 8 | 9 | #ifndef INPUT_MANAGER_H 10 | #define INPUT_MANAGER_H 11 | 12 | #include // only for arm_state_t 13 | #include 14 | 15 | /** 16 | * @brief determines how the dsm radio indicates an arm/disarm kill switch 17 | */ 18 | typedef enum dsm_kill_mode_t 19 | { 20 | /** 21 | * A dedicated channel is used as a kill switch. Carefully set the 22 | * dsm_kill_ch and dsm_kill_pol channel and polarity settings. 23 | */ 24 | DSM_KILL_DEDICATED_SWITCH, 25 | /** 26 | * Some radios, such as Spektrum DXe have an ARM/DISARM switch which 27 | * forces the throttle channel down below normal range to disarm. This 28 | * frees up a channel for other use and is the preffered method. When 29 | * using this mode, dsm_kill_ch and dsm_kill_pol are ignored. 30 | */ 31 | DSM_KILL_NEGATIVE_THROTTLE 32 | } dsm_kill_mode_t; 33 | 34 | /** 35 | * Represents current command by the user. This is populated by the 36 | * input_manager thread which decides to read from mavlink or DSM depending on 37 | * what it is receiving. 38 | */ 39 | typedef struct user_input_t 40 | { 41 | int initialized; ///< set to 1 after input_manager_init(void) 42 | flight_mode_t flight_mode; ///< this is the user commanded flight_mode. 43 | int input_active; ///< nonzero indicates some user control is coming in 44 | arm_state_t requested_arm_mode; ///< set to ARMED after arming sequence is entered. 45 | 46 | // All sticks scaled from -1 to 1 47 | double thr_stick; ///< positive forward 48 | double yaw_stick; ///< positive to the right, CW yaw 49 | double roll_stick; ///< positive to the right 50 | double pitch_stick; ///< positive forward 51 | } user_input_t; 52 | 53 | extern user_input_t user_input; 54 | 55 | /** 56 | * @brief Starts an input manager thread. 57 | * 58 | * Watch for new DSM data and translate into local user mode. Used 59 | * in input_manager.c 60 | * 61 | * @return 0 on success, -1 on failure 62 | */ 63 | int input_manager_init(void); 64 | 65 | /** 66 | * @brief Waits for the input manager thread to exit 67 | * 68 | * This should only be called after the program flow state is set to 69 | * EXITING as that's the only thing that will cause the thread to 70 | * exit on its own safely. Used in input_manager.c 71 | * 72 | * @return 0 on clean exit, -1 if exit timed out. 73 | */ 74 | int input_manager_cleanup(void); 75 | 76 | #endif // INPUT_MANAGER_H 77 | -------------------------------------------------------------------------------- /include/log_manager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * @brief Functions to start, stop, and interact with the log manager 5 | * thread. 6 | * 7 | */ 8 | 9 | #ifndef LOG_MANAGER_H 10 | #define LOG_MANAGER_H 11 | 12 | /** 13 | * Struct containing all possible values that could be writen to the log. For 14 | * each log entry you wish to create, fill in an instance of this and pass to 15 | * add_log_entry(void). You do not need to populate all parts of the struct. 16 | * Currently feedback.c populates all values and log_manager.c only writes the 17 | * values enabled in the settings file. 18 | */ 19 | typedef struct log_entry_t 20 | { 21 | /** @name index, always printed */ 22 | ///@{ 23 | uint64_t loop_index; // timing 24 | uint64_t last_step_ns; 25 | ///@} 26 | 27 | /** @name sensors */ 28 | ///@{ 29 | double v_batt; 30 | double alt_bmp_raw; 31 | double gyro_roll; 32 | double gyro_pitch; 33 | double gyro_yaw; 34 | double accel_X; 35 | double accel_Y; 36 | double accel_Z; 37 | ///@} 38 | 39 | /** @name state estimate */ 40 | ///@{ 41 | double roll; 42 | double pitch; 43 | double yaw; 44 | double X; 45 | double Y; 46 | double Z; 47 | double Xdot; 48 | double Ydot; 49 | double Zdot; 50 | ///@} 51 | 52 | /** @name setpoint */ 53 | ///@{ 54 | double sp_roll; 55 | double sp_pitch; 56 | double sp_yaw; 57 | double sp_X; 58 | double sp_Y; 59 | double sp_Z; 60 | double sp_Xdot; 61 | double sp_Ydot; 62 | double sp_Zdot; 63 | ///@} 64 | 65 | /** @name orthogonal control outputs */ 66 | ///@{ 67 | double u_roll; 68 | double u_pitch; 69 | double u_yaw; 70 | double u_X; 71 | double u_Y; 72 | double u_Z; 73 | ///@} 74 | 75 | /** @name motor signals */ 76 | ///@{ 77 | double mot_1; 78 | double mot_2; 79 | double mot_3; 80 | double mot_4; 81 | double mot_5; 82 | double mot_6; 83 | double mot_7; 84 | double mot_8; 85 | ///@} 86 | 87 | } log_entry_t; 88 | 89 | /** 90 | * @brief creates a new csv log file and starts the background thread. 91 | * 92 | * @return 0 on success, -1 on failure 93 | */ 94 | int log_manager_init(void); 95 | 96 | /** 97 | * @brief quickly add new data to local buffer 98 | * 99 | * This is called after feedback_march after signals have been sent to 100 | * the motors. 101 | * 102 | * @return 0 on success, -1 on failure 103 | */ 104 | int log_manager_add_new(); 105 | 106 | /** 107 | * @brief Finish writing remaining data to log and close thread. 108 | * 109 | * Used in log_manager.c 110 | * 111 | * @return 0 on sucess and clean exit, -1 on exit timeout/force close. 112 | */ 113 | int log_manager_cleanup(void); 114 | 115 | #endif // LOG_MANAGER_H 116 | -------------------------------------------------------------------------------- /include/mavlink_manager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * Functions to start and stop the mavlink manager 5 | */ 6 | 7 | #ifndef MAVLINK_MANAGER_H 8 | #define MAVLINK_MANAGER_H 9 | 10 | /** 11 | * @brief Starts the mavlink manager 12 | * 13 | * @return 0 on success, -1 on failure 14 | */ 15 | int mavlink_manager_init(void); 16 | 17 | /** 18 | * @brief stops the mavlink manager 19 | * 20 | * @return 0 if thread exited cleanly, -1 if exit timed out. 21 | */ 22 | int mavlink_manager_cleanup(void); 23 | 24 | #endif // MAVLINK_MANAGER_H 25 | -------------------------------------------------------------------------------- /include/mix.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * @brief Functions to mix orthogonal inputs to motor controls 5 | * 6 | * MultiRotors are controlled by mixing roll, pitch, yaw, and 7 | * throttle control oututs, a linear combination of which forms the 8 | * control output to each motor. The coefficients to this 9 | * combination is stored in a mixing matrix based on rotor layout. 10 | * Also included here are functions to parse configuration strings 11 | * and do the actual mixing. 12 | */ 13 | 14 | #ifndef MIXING_MATRIX_H 15 | #define MIXING_MATRIX_H 16 | 17 | #define MAX_INPUTS 6 ///< up to 6 control inputs (roll,pitch,yaw,z,x,y) 18 | #define MAX_ROTORS 8 ///< up to 8 rotors 19 | 20 | /** 21 | * @brief enum for possible mixing matrices defined here 22 | * 23 | * possible rotor configurations, see mixing_matrix_defs.h 24 | */ 25 | typedef enum rotor_layout_t 26 | { 27 | LAYOUT_4X, 28 | LAYOUT_4PLUS, 29 | LAYOUT_6X, 30 | LAYOUT_8X, 31 | LAYOUT_6DOF_ROTORBITS, 32 | LAYOUT_6DOF_5INCH_MONOCOQUE 33 | } rotor_layout_t; 34 | 35 | /** 36 | * @brief Initiallizes the mixing matrix for a given input layout. 37 | * 38 | * For a given number of rotors, layout character ('X','+') and 39 | * degrees of freedom in control input (4 or 6), this selects the 40 | * correct predefined mixing matrix from mixing_matrix_defs.h. The 41 | * matrix is kept locally in mixing _matrix.c to prevent accidental 42 | * misuse or modification. Use the other function from 43 | * mixing_matrix.c below to interface with it. Used in 44 | * mixing_matrix.c 45 | * 46 | * @param[in] layout The layout enum 47 | * 48 | * @return 0 on success, -1 on failure 49 | */ 50 | int mix_init(rotor_layout_t layout); 51 | 52 | /** 53 | * @brief Fills the vector mot with the linear combination of XYZ, roll 54 | * pitch yaw. Not actually used, only for testing. 55 | * 56 | * Fills the vector mot with the linear combination of roll pitch 57 | * yaw and throttle based on mixing matrix. If dof = 6, X and Y are 58 | * also added. Outputs are blindly saturated between 0 and 1. This 59 | * is for rudimentary mixing and testing only. It is recommended to 60 | * check for saturation for each input with check_channel_saturation 61 | * then add inputs sequentially with add_mixed_input() instead. Used 62 | * in mixing_matrix.c 63 | * 64 | * @param u 6 control inputs 65 | * @param mot pointer to motor outputs 66 | * 67 | * @return 0 on success, -1 on failure 68 | */ 69 | int mix_all_controls(double u[6], double* mot); 70 | 71 | /** 72 | * @brief Finds the min and max inputs u that can be applied to a current 73 | * set of motor outputs before saturating any one motor. 74 | * 75 | * This is a precurser check to be done before marching a feedback 76 | * controller forward so we know what to saturate the transfer 77 | * function output at. Outputs min and max are given as pointers 78 | * that are written back to. The mot motor array argument is the 79 | * array of current motor outputs that the new channel ch will be 80 | * adding onto. 81 | * 82 | * @param[in] ch channel 83 | * @param[in] mot The array of motor signals to be added onto 84 | * @param[out] min The minimum possible input without saturation 85 | * @param[out] max The maximum possible input without saturation 86 | * 87 | * @return 0 on success, -1 on failure 88 | */ 89 | int mix_check_saturation(int ch, double* mot, double* min, double* max); 90 | 91 | /** 92 | * @brief Mixes the control input u for a single channel ch to the existing 93 | * motor array mot. 94 | * 95 | * Mixes the control input u for a single channel ch corresponding 96 | * to throttle roll pitch etc to the existing motor array mot. No 97 | * saturation is done, the input u should be checked for saturation 98 | * validity with check_channel_saturation() first. Used in 99 | * mixing_matrix.c 100 | * 101 | * @param[in] u control input 102 | * @param[in] ch channel 103 | * @param mot array of motor channels 104 | * 105 | * @return 0 on success, -1 on failure 106 | */ 107 | int mix_add_input(double u, int ch, double* mot); 108 | 109 | #endif // MIXING_MATRIX_H -------------------------------------------------------------------------------- /include/printf_manager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * @brief Functions to start and stop the printf mnaager which is a 5 | * separate thread printing data to the console for debugging. 6 | */ 7 | 8 | #ifndef PRINTF_MANAGER_H 9 | #define PRINTF_MANAGER_H 10 | 11 | #include 12 | 13 | /** 14 | * @brief Start the printf_manager thread which should be the only thing 15 | * printing to the screen besides error messages from other threads. 16 | * 17 | * @return 0 on success, -1 on failure 18 | */ 19 | int printf_init(void); 20 | 21 | /** 22 | * @brief Waits for the printf manager thread to exit. 23 | * 24 | * @return 0 on clean exit, -1 on exit time out/force close 25 | */ 26 | int printf_cleanup(void); 27 | 28 | /** 29 | * @brief Only used by printf_manager right now, but could be useful 30 | * elsewhere. 31 | * 32 | * @param[in] mode The mode 33 | * 34 | * @return 0 on success or -1 on error 35 | */ 36 | int print_flight_mode(flight_mode_t mode); 37 | 38 | #endif // PRINTF_MANAGER_H 39 | -------------------------------------------------------------------------------- /include/rc_pilot_defs.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * @brief constants and parameters 5 | */ 6 | 7 | #ifndef RC_PILOT_DEFS_H 8 | #define RC_PILOT_DEFS_H 9 | 10 | /** 11 | * @brief ARMED or DISARMED to indicate if the feedback controller is 12 | * allowed to output to the motors 13 | */ 14 | typedef enum arm_state_t 15 | { 16 | DISARMED, 17 | ARMED 18 | } arm_state_t; 19 | 20 | // Speed of feedback loop 21 | #define FEEDBACK_HZ 200 22 | #define DT 0.005 23 | 24 | // IMU Parameters 25 | #define IMU_PRIORITY 51 26 | #define I2C_BUS 2 27 | #define GPIO_INT_PIN_CHIP 3 28 | #define GPIO_INT_PIN_PIN 21 29 | 30 | // top safety 31 | #define ARM_TIP_THRESHOLD 0.2 ///< radians from level to allow arming sequence 32 | #define TIP_ANGLE 1.5 ///< radiands of roll or pitch to consider tipped over 33 | 34 | // math constants 35 | #define GRAVITY 9.80665 ///< one G m/s^2 36 | 37 | // order of control inputs 38 | // throttle(Z), roll, pitch, YAW, sideways (X),forward(Y) 39 | #define VEC_X 0 40 | #define VEC_Y 1 41 | #define VEC_Z 2 42 | #define VEC_ROLL 3 43 | #define VEC_PITCH 4 44 | #define VEC_YAW 5 45 | 46 | // user control parameters 47 | #define MAX_YAW_RATE 2.5 // rad/s 48 | #define MAX_ROLL_SETPOINT 0.2 // rad 49 | #define MAX_PITCH_SETPOINT 0.2 // rad 50 | #define MAX_CLIMB_RATE 1.0 // m/s 51 | #define YAW_DEADZONE 0.02 52 | #define THROTTLE_DEADZONE 0.02 53 | #define SOFT_START_SECONDS 1.0 // controller soft start seconds 54 | #define ALT_CUTOFF_FREQ 2.0 55 | #define BMP_RATE_DIV 10 // optionally sample bmp less frequently than mpu 56 | 57 | // controller absolute limits 58 | #define MAX_ROLL_COMPONENT 0.4 59 | #define MAX_PITCH_COMPONENT 0.4 60 | #define MAX_YAW_COMPONENT 0.4 61 | #define MAX_X_COMPONENT 1.0 62 | #define MAX_Y_COMPONENT 1.0 63 | /** 64 | * MAX_THRUST_COMPONENT is really "lowest power state" or idle value. Note that 65 | * after the thrust mapping a different value will actually be sent to the motors. 66 | * The sign is inverted because these are control values in NED coordinates 67 | */ 68 | #define MAX_THRUST_COMPONENT -0.05 69 | #define MIN_THRUST_COMPONENT -0.75 70 | 71 | // Files 72 | #define LOG_DIR "/home/debian/rc_pilot_logs/" 73 | 74 | // for future modes, not used yet 75 | #define LAND_TIMEOUT 0.3 76 | #define DISARM_TIMEOUT 4.0 77 | 78 | // terminal emulator control sequences 79 | #define WRAP_DISABLE "\033[?7l" 80 | #define WRAP_ENABLE "\033[?7h" 81 | #define KNRM "\x1B[0m" // "normal" to return to default after colour 82 | #define KRED "\x1B[31m" 83 | #define KGRN "\x1B[32m" 84 | #define KYEL "\x1B[33m" 85 | #define KBLU "\x1B[34m" 86 | #define KMAG "\x1B[35m" 87 | #define KCYN "\x1B[36m" 88 | #define KWHT "\x1B[37m" 89 | 90 | //#define DEBUG 91 | 92 | #endif // RC_PILOT_DEFS_H -------------------------------------------------------------------------------- /include/setpoint_manager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * @brief Setpoint manager runs at the same rate as the feedback controller 5 | * and is the interface between the user inputs (input manager) and 6 | * the feedback controller setpoint. currently it contains very 7 | * simply logic and runs very quickly which is why it's okay to run 8 | * in the feedback ISR right before the feedback controller. In the 9 | * future this is where go-home and other higher level autonomy will 10 | * live. 11 | * 12 | * This serves to allow the feedback controller to be as simple and 13 | * clean as possible by putting all high-level manipulation of the 14 | * setpoints here. Then feedback-controller only needs to march the 15 | * filters and zero them out when arming or enabling controllers 16 | */ 17 | 18 | #ifndef SETPOINT_MANAGER_H 19 | #define SETPOINT_MANAGER_H 20 | 21 | #include 22 | 23 | /** 24 | * Setpoint for the feedback controllers. This is written by setpoint_manager 25 | * and primarily read in by fly_controller. May also be read by printf_manager 26 | * and log_manager for telemetry 27 | */ 28 | typedef struct setpoint_t 29 | { 30 | /** @name general */ 31 | ///< @{ 32 | int initialized; ///< set to 1 once setpoint manager has initialized 33 | int en_6dof; ///< enable 6DOF control features 34 | ///< @} 35 | 36 | /** @name direct passthrough 37 | * user inputs tranlate directly to mixing matrix 38 | */ 39 | ///< @{ 40 | double Z_throttle; ///< used only when altitude controller disabled 41 | double X_throttle; ///< only used when 6dof is enabled, positive forward 42 | double Y_throttle; ///< only used when 6dof is enabled, positive right 43 | double roll_throttle; ///< only used when roll_pitch_yaw controllers are disbaled 44 | double pitch_throttle; ///< only used when roll_pitch_yaw controllers are disbaled 45 | double yaw_throttle; ///< only used when roll_pitch_yaw controllers are disbaled 46 | ///< @} 47 | 48 | /** @name attitude setpoint */ 49 | ///< @{ 50 | int en_rpy_ctrl; ///< enable the roll pitch yaw controllers 51 | double roll; ///< roll angle (positive tip right) (rad) 52 | double pitch; ///< pitch angle (positive tip back) (rad) 53 | double yaw; ///< glabal yaw angle, positive left 54 | double yaw_dot; ///< desired rate of change in yaw rad/s 55 | ///< @} 56 | 57 | /** @name altitude */ 58 | ///< @{ 59 | int en_Z_ctrl; ///< enable altitude feedback. 60 | double Z; ///< vertical distance from where controller was armed 61 | double Z_dot; ///< vertical velocity m/s^2, remember Z points down 62 | ///< @} 63 | 64 | /** @name horizontal velocity setpoint */ 65 | ///< @{ 66 | int en_XY_vel_ctrl; 67 | double X_dot; 68 | double Y_dot; 69 | ///< @} 70 | 71 | /** @name horizontal velocity setpoint */ 72 | ///< @{ 73 | int en_XY_pos_ctrl; 74 | double X; 75 | double Y; 76 | } setpoint_t; 77 | 78 | extern setpoint_t setpoint; 79 | 80 | /** 81 | * @brief Initializes the setpoint manager. 82 | * 83 | * @return 0 on success, -1 on failure 84 | */ 85 | int setpoint_manager_init(void); 86 | 87 | /** 88 | * @brief updates the setpoint manager, call this before feedback loop 89 | * 90 | * @return 0 on success, -1 on failure 91 | */ 92 | int setpoint_manager_update(void); 93 | 94 | /** 95 | * @brief cleans up the setpoint manager, not really necessary but here for 96 | * completeness 97 | * 98 | * @return 0 on clean exit, -1 if exit timed out 99 | */ 100 | int setpoint_manager_cleanup(void); 101 | 102 | #endif // SETPOINT_MANAGER_H 103 | -------------------------------------------------------------------------------- /include/settings.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * 4 | * @brief Functions to read the json settings file 5 | */ 6 | 7 | #ifndef SETTINGS_H 8 | #define SETTINGS_H 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | /** 20 | * Configuration settings read from the json settings file and passed to most 21 | * threads as they initialize. 22 | */ 23 | typedef struct settings_t 24 | { 25 | /** @name File details */ 26 | char name[128]; ///< string declaring the name of the settings file 27 | ///@} 28 | 29 | /**@name warings */ 30 | ///@{ 31 | int warnings_en; 32 | ///@} 33 | 34 | /** @name physical parameters */ 35 | ///@{ 36 | int num_rotors; 37 | rotor_layout_t layout; 38 | int dof; 39 | thrust_map_t thrust_map; 40 | double v_nominal; 41 | int enable_magnetometer; // we suggest leaving as 0 (mag OFF) 42 | ///@} 43 | 44 | /** @name flight modes */ 45 | ///@{ 46 | int num_dsm_modes; 47 | flight_mode_t flight_mode_1; 48 | flight_mode_t flight_mode_2; 49 | flight_mode_t flight_mode_3; 50 | ///@} 51 | 52 | /** @name dsm radio config */ 53 | ///@{ 54 | int dsm_thr_ch; 55 | int dsm_thr_pol; 56 | int dsm_roll_ch; 57 | int dsm_roll_pol; 58 | int dsm_pitch_ch; 59 | int dsm_pitch_pol; 60 | int dsm_yaw_ch; 61 | int dsm_yaw_pol; 62 | int dsm_mode_ch; 63 | int dsm_mode_pol; 64 | dsm_kill_mode_t dsm_kill_mode; 65 | int dsm_kill_ch; 66 | int dsm_kill_pol; 67 | ///@} 68 | 69 | /** @name printf settings */ 70 | ///@{ 71 | int printf_arm; 72 | int printf_altitude; 73 | int printf_rpy; 74 | int printf_sticks; 75 | int printf_setpoint; 76 | int printf_u; 77 | int printf_motors; 78 | int printf_mode; 79 | ///@} 80 | 81 | /** @name log settings */ 82 | ///@{ 83 | int enable_logging; 84 | int log_sensors; 85 | int log_state; 86 | int log_setpoint; 87 | int log_control_u; 88 | int log_motor_signals; 89 | ///@} 90 | 91 | /** @name mavlink stuff */ 92 | ///@{ 93 | char dest_ip[24]; 94 | uint8_t my_sys_id; 95 | uint16_t mav_port; 96 | 97 | /** @name feedback controllers */ 98 | ///@{ 99 | rc_filter_t roll_controller; 100 | rc_filter_t pitch_controller; 101 | rc_filter_t yaw_controller; 102 | rc_filter_t altitude_controller; 103 | rc_filter_t horiz_vel_ctrl_4dof; 104 | rc_filter_t horiz_vel_ctrl_6dof; 105 | rc_filter_t horiz_pos_ctrl_4dof; 106 | rc_filter_t horiz_pos_ctrl_6dof; 107 | double max_XY_velocity; 108 | double max_Z_velocity; 109 | ///@} 110 | 111 | } settings_t; 112 | 113 | /** 114 | * settings are external, so just include this header and read from it 115 | */ 116 | extern settings_t settings; 117 | 118 | /** 119 | * @brief Populates the settings and controller structs with the settings file. 120 | * 121 | * @return 0 on success, -1 on failure 122 | */ 123 | int settings_load_from_file(char* path); 124 | 125 | /** 126 | * @brief Only used in debug mode. Prints settings to console 127 | * 128 | * @return 0 on success, -1 on failure 129 | */ 130 | int settings_print(void); 131 | 132 | #endif // SETTINGS_H 133 | -------------------------------------------------------------------------------- /include/state_estimator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file state_estimator.h 3 | * 4 | * @brief Functions to start and stop the state estimator 5 | * 6 | * This runs at the same rate as the feedback controller. 7 | * state_estimator_march() is called immediately before feedback_march() in the 8 | * IMU interrupt service routine. 9 | */ 10 | 11 | #ifndef STATE_ESTIMATOR_H 12 | #define STATE_ESTIMATOR_H 13 | 14 | #include 15 | #include 16 | #include // for uint64_t 17 | 18 | /** 19 | * This is the output from the state estimator. It contains raw sensor values 20 | * and the outputs of filters. Everything is in NED coordinates defined as: 21 | * 22 | * - X pointing Rorward 23 | * - Y pointing Right 24 | * - Z pointing Down 25 | * 26 | * right hand rule applies for angular values such as tait bryan angles and gyro 27 | * - Positive Roll to the right about X 28 | * - Positive Pitch back about Y 29 | * - Positive Yaw right about Z 30 | */ 31 | typedef struct state_estimate_t 32 | { 33 | int initialized; 34 | 35 | /** @name IMU (accel gyro) 36 | * Normalized Quaternion is straight from the DMP but converted to NED 37 | * coordinates. Tait-Bryan angles roll pitch and yaw angles are then 38 | * converted from the quaternion. 39 | * the roll_pitch_yaw values in the taid bryan angles tb_imu are bounded 40 | * by +-pi since they come straight from the quaternion. the state estimator 41 | * keeps track of these rotations and generates continuous_yaw which is 42 | * unbounded and keeps track of multiple rotations. This provides a continuously 43 | * differentiable variable with no jumps between +-pi 44 | */ 45 | ///@{ 46 | double gyro[3]; ///< gyro roll pitch yaw (rad/s) 47 | double accel[3]; ///< accel XYZ NED coordinates (m/s^2) 48 | double quat_imu[4]; ///< DMP normalized quaternion 49 | double tb_imu[3]; ///< tait bryan roll pitch yaw angle (rad) 50 | double imu_continuous_yaw; ///< continuous yaw from imu only (multiple turns) 51 | ///@} 52 | 53 | /** @name IMU (magnetometer) 54 | * these values are only set when magnetometer is enabled in settings. 55 | * right now these aren't used and we don't suggest turning the magnetometer on 56 | */ 57 | ///@{ 58 | double mag[3]; ///< magnetometer XYZ NED coordinates () 59 | double mag_heading_raw; ///< raw compass heading 60 | double mag_heading; ///< compass heading filtered with IMU 61 | double mag_heading_continuous; 62 | double quat_mag[4]; ///< quaterion filtered 63 | double tb_mag[3]; ///< roll pitch yaw with magetometer heading fixed (rad) 64 | ///@} 65 | 66 | /** @name selected values for feedback 67 | * these are copoies of other values in this state estimate used for feedback 68 | * this is done so we can easily chose which source to get feedback from (mag or no mag) 69 | */ 70 | ///@{ 71 | double roll; 72 | double pitch; 73 | double yaw; 74 | double continuous_yaw; ///< keeps increasing/decreasing aboce +-2pi 75 | double X; 76 | double Y; 77 | double Z; 78 | ///@} 79 | 80 | /** @name filtered data from IMU & barometer 81 | * Altitude estimates from kalman filter fusing IMU and BMP data. 82 | * Alttitude, velocity, and acceleration are in units of m, m/s, m/s^2 83 | * Note this is altitude so positive is upwards unlike the NED 84 | * coordinate frame that has Z pointing down. 85 | */ 86 | ///@{ 87 | double bmp_pressure_raw; ///< raw barometer pressure in Pascals 88 | double alt_bmp_raw; ///< altitude estimate using only bmp from sea level (m) 89 | double alt_bmp; ///< altitude estimate using kalman filter (IMU & bmp) 90 | double alt_bmp_vel; ///< z velocity estimate using kalman filter (IMU & bmp) 91 | double alt_bmp_accel; ///< z accel estimate using kalman filter (IMU & bmp) 92 | ///@} 93 | 94 | /** @name Motion Capture data 95 | * As mocap drop in and out the mocap_running flag will turn on and off. 96 | * Old values will remain readable after mocap drops out. 97 | */ 98 | ///@{ 99 | int mocap_running; ///< 1 if motion capture data is recent and valid 100 | uint64_t mocap_timestamp_ns; ///< timestamp of last received packet in nanoseconds since boot 101 | double pos_mocap[3]; ///< position in mocap frame, converted to NED if necessary 102 | double quat_mocap[4]; ///< UAV orientation according to mocap 103 | double tb_mocap[3]; ///< Tait-Bryan angles according to mocap 104 | int is_active; ///< TODO used by mavlink manager, purpose unclear... (pg) 105 | ///@} 106 | 107 | /** @name Global Position Estimate 108 | * This is the global estimated position, velocity, and acceleration 109 | * output of a kalman filter which filters accelerometer, DMP attitude, 110 | * and mocap data. If mocap is not available, barometer will be used. 111 | * 112 | * global values are in the mocap's frame for position control. 113 | * relative values are in a frame who's origin is at the position where 114 | * the feedback controller is armed. Without mocap data the filter will 115 | * assume altitude from the barometer and accelerometer, and position 116 | * estimate will have steady state gain of zero to prevent runaway. 117 | */ 118 | ///@{ 119 | double pos_global[3]; 120 | double vel_global[3]; 121 | double accel_global[3]; 122 | double pos_relative[3]; 123 | double vel_relative[3]; 124 | double accel_relative[3]; 125 | ///@} 126 | 127 | /** @name Other */ 128 | ///@{ 129 | double v_batt_raw; ///< main battery pack voltage (v) 130 | double v_batt_lp; ///< main battery pack voltage with low pass filter (v) 131 | double bmp_temp; ///< temperature of barometer 132 | ///@} 133 | 134 | } state_estimate_t; 135 | 136 | extern state_estimate_t state_estimate; 137 | extern rc_mpu_data_t mpu_data; 138 | 139 | /** 140 | * @brief Initial setup of the state estimator 141 | * 142 | * barometer must be initialized first 143 | * 144 | * @return 0 on success, -1 on failure 145 | */ 146 | int state_estimator_init(void); 147 | 148 | /** 149 | * @brief March state estimator forward one step 150 | * 151 | * Called immediately before feedback_march 152 | * 153 | * @return 0 on success, -1 on failure 154 | */ 155 | int state_estimator_march(void); 156 | 157 | /** 158 | * @brief jobs the state estimator must do after feedback_controller 159 | * 160 | * Called immediately after feedback_march in the ISR. Currently this reads 161 | * 162 | * @return 0 on success, -1 on failure 163 | */ 164 | int state_estimator_jobs_after_feedback(void); 165 | 166 | /** 167 | * @brief Cleanup the state estimator, freeing memory 168 | * 169 | * @return 0 on success, -1 on failure 170 | */ 171 | int state_estimator_cleanup(void); 172 | 173 | #endif // STATE_ESTIMATOR_H 174 | -------------------------------------------------------------------------------- /include/thread_defs.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef THREAD_DEFS_H 4 | #define THREAD_DEFS_H 5 | 6 | // thread speeds, prioritites, and close timeouts 7 | #define INPUT_MANAGER_HZ 20 8 | #define INPUT_MANAGER_PRI 80 9 | #define INPUT_MANAGER_TOUT 0.5 10 | #define LOG_MANAGER_HZ 20 11 | #define LOG_MANAGER_PRI 50 12 | #define LOG_MANAGER_TOUT 2.0 13 | #define PRINTF_MANAGER_HZ 20 14 | #define PRINTF_MANAGER_PRI 60 15 | #define PRINTF_MANAGER_TOUT 0.5 16 | #define BUTTON_EXIT_CHECK_HZ 10 17 | #define BUTTON_EXIT_TIME_S 2 18 | 19 | #endif -------------------------------------------------------------------------------- /include/thrust_map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @headerfile thrust_map.h 3 | * 4 | * @brief Functions to start and stop the printf manager which is a 5 | * separate thread printing data to the console for debugging. 6 | */ 7 | 8 | #ifndef THRUST_MAP_H 9 | #define THRUST_MAP_H 10 | 11 | /** 12 | * enum thrust_map_t 13 | * 14 | * the user may select from the following preconfigured thrust maps 15 | */ 16 | typedef enum thrust_map_t 17 | { 18 | LINEAR_MAP, 19 | MN1806_1400KV_4S, 20 | F20_2300KV_2S, 21 | RX2206_4S 22 | } thrust_map_t; 23 | 24 | /** 25 | * @brief Check the thrust map for validity and populate data arrays. 26 | * 27 | * @return 0 on success, -1 on failure 28 | */ 29 | int thrust_map_init(thrust_map_t map); 30 | 31 | /** 32 | * @brief Corrects the motor signal m for non-linear thrust curve in place. 33 | * 34 | * 35 | * @param[in] m thrust input, must be between 0 and 1 inclusive 36 | * 37 | * @return motor signal value on success, -1 on error 38 | */ 39 | double map_motor_signal(double m); 40 | 41 | #endif // THRUST_MAP_H 42 | -------------------------------------------------------------------------------- /scripts/copy_logs: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | main(){ 4 | if [ $# -eq 1 ]; then 5 | if [ $1 = usb ]; then 6 | LOC=debian@192.168.6.2 7 | elif [ $1 = wifi ]; then 8 | LOC=debian@192.168.8.1 9 | else 10 | usage_error 11 | fi 12 | else 13 | usage_error 14 | fi 15 | 16 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 17 | LOG_FILE=$SCRIPT_DIR/../../flight_logs/flight_logs_$(date +%F_%H%M%S) 18 | 19 | mkdir -p $LOG_FILE 20 | 21 | rsync -avzh --progress $LOC:/home/debian/rc_pilot_logs/ $LOG_FILE 22 | } 23 | 24 | usage_error(){ 25 | echo "ERROR: Please specify beaglebone connection" 26 | echo "Usage: ./transfer_rcpilot [usb|wifi]" 27 | exit 1 28 | } 29 | 30 | main "$@" -------------------------------------------------------------------------------- /scripts/transfer_rcpilot: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | main() { 4 | # Parse input to determine the destination IP 5 | if [ $# -eq 1 ]; then 6 | if [ $1 = usb ]; then 7 | DEST=debian@192.168.6.2 8 | elif [ $1 = wifi ]; then 9 | DEST=debian@192.168.8.1 10 | else 11 | usage_error 12 | fi 13 | else 14 | usage_error 15 | fi 16 | 17 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 18 | RC_PILOT_DIR=$SCRIPT_DIR/.. 19 | BBB_DEST=$DEST:/home/debian/rc_pilot/ 20 | TMP_FILE=/tmp/include_files 21 | 22 | # Folders and files to transfer to the beaglebone 23 | INCLUDE_FILES='include/ src/ Makefile settings/' 24 | 25 | # Populates tmp_file with all of the files to send to the beaglebone 26 | # (needed by the --include-from rsync flag) 27 | find $INCLUDE_FILES > $TMP_FILE 28 | 29 | # Set the date on the beaglebone to mach source computer, eliminates clock skew on make 30 | DATE=$(date) 31 | SET_DATE="echo 'temppwd' | sudo -S date -s ""'""$DATE""'" 32 | ssh -t $DEST $SET_DATE 33 | 34 | # Transfer the files to the beaglebone. Performs archiving operation that only updates files 35 | # that have newer changes on the tranfering computer. Requires the date to be set properly for this 36 | # to work. 37 | rsync -avzh --include-from $TMP_FILE --exclude '*' --progress $RC_PILOT_DIR $BBB_DEST 38 | } 39 | 40 | function usage_error() { 41 | echo "ERROR: Please specify beaglebone connection" 42 | echo "Usage: ./transfer_rcpilot [usb|wifi]" 43 | exit 1 44 | } 45 | 46 | main "$@" 47 | -------------------------------------------------------------------------------- /settings/hex_6dof_settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "HEX 6DOF JAMES", 3 | 4 | "warnings_en": 1, 5 | 6 | "layout": "LAYOUT_6DOF_ROTORBITS", 7 | "thrust_map": "RX2206_4S", 8 | "orientation": "ORIENTATION_X_FORWARD", 9 | "v_nominal": 14.8, 10 | "enable_magnetometer": false, 11 | 12 | "num_dsm_modes": 3, 13 | "flight_mode_1": "TEST_BENCH_4DOF", 14 | "flight_mode_2": "DIRECT_THROTTLE_4DOF", 15 | "flight_mode_3": "DIRECT_THROTTLE_6DOF", 16 | 17 | "dsm_thr_ch": 1, 18 | "dsm_thr_pol": 1, 19 | "dsm_roll_ch": 2, 20 | "dsm_roll_pol": -1, 21 | "dsm_pitch_ch": 3, 22 | "dsm_pitch_pol": -1, 23 | "dsm_yaw_ch": 4, 24 | "dsm_yaw_pol": -1, 25 | "dsm_mode_ch": 5, 26 | "dsm_mode_pol": -1, 27 | "dsm_kill_mode": "DSM_KILL_NEGATIVE_THROTTLE", 28 | "dsm_kill_ch": 6, 29 | "dsm_kill_pol": 1, 30 | 31 | "printf_arm": true, 32 | "printf_altitude": true, 33 | "printf_position": true, 34 | "printf_rpy": true, 35 | "printf_sticks": true, 36 | "printf_setpoint": true, 37 | "printf_u": true, 38 | "printf_motors": true, 39 | "printf_mode": true, 40 | 41 | "enable_logging": false, 42 | "log_sensors": true, 43 | "log_state": true, 44 | "log_setpoint": true, 45 | "log_control_u": true, 46 | "log_motor_signals": true, 47 | 48 | "dest_ip": "192.168.8.1", 49 | "my_sys_id": 1, 50 | "mav_port": 14551, 51 | 52 | "roll_controller": { 53 | "gain": 1.0, 54 | "CT_or_DT": "CT", 55 | "TF_or_PID": "PID", 56 | "kp":0.3, 57 | "ki":0.01, 58 | "kd":0.1, 59 | "crossover_freq_rad_per_sec": 62.83, 60 | "numerator": [ 61 | 0.1, 62 | 0.2, 63 | 0.3 64 | ], 65 | "denominator": [ 66 | 0.1, 67 | 0.2, 68 | 0.3 69 | ] 70 | }, 71 | 72 | "pitch_controller": { 73 | "gain": 1.0, 74 | "CT_or_DT": "CT", 75 | "TF_or_PID": "PID", 76 | "kp":0.3, 77 | "ki":0.01, 78 | "kd":0.1, 79 | "crossover_freq_rad_per_sec": 62.83, 80 | "numerator": [ 81 | 0.1, 82 | 0.2, 83 | 0.3 84 | ], 85 | "denominator": [ 86 | 0.1, 87 | 0.2, 88 | 0.3 89 | ] 90 | }, 91 | 92 | "yaw_controller": { 93 | "gain": 1.0, 94 | "CT_or_DT": "CT", 95 | "TF_or_PID": "PID", 96 | "kp":0.3, 97 | "ki":0.01, 98 | "kd":0.1, 99 | "crossover_freq_rad_per_sec": 31.41, 100 | "numerator": [ 101 | 0.1, 102 | 0.2, 103 | 0.3 104 | ], 105 | "denominator": [ 106 | 0.1, 107 | 0.2, 108 | 0.3 109 | ] 110 | }, 111 | 112 | "altitude_controller": { 113 | "gain": 1.0, 114 | "CT_or_DT": "CT", 115 | "TF_or_PID": "PID", 116 | "kp":0.85, 117 | "ki":0.0, 118 | "kd":0.0, 119 | "crossover_freq_rad_per_sec": 6.283, 120 | "numerator": [ 121 | 0.1, 122 | 0.2, 123 | 0.3 124 | ], 125 | "denominator": [ 126 | 0.1, 127 | 0.2, 128 | 0.3 129 | ] 130 | }, 131 | 132 | "horizontal_velocity_controller_4dof": { 133 | "gain": 1.0, 134 | "CT_or_DT": "CT", 135 | "TF_or_PID": "PID", 136 | "kp":0.85, 137 | "ki":0.0, 138 | "kd":0.0, 139 | "crossover_freq_rad_per_sec": 6.283, 140 | "numerator": [ 141 | 0.1, 142 | 0.2, 143 | 0.3 144 | ], 145 | "denominator": [ 146 | 0.1, 147 | 0.2, 148 | 0.3 149 | ] 150 | }, 151 | 152 | "horizontal_velocity_controller_6dof": { 153 | "gain": 1.0, 154 | "CT_or_DT": "CT", 155 | "TF_or_PID": "PID", 156 | "kp":0.85, 157 | "ki":0.0, 158 | "kd":0.0, 159 | "crossover_freq_rad_per_sec": 6.283, 160 | "numerator": [ 161 | 0.1, 162 | 0.2, 163 | 0.3 164 | ], 165 | "denominator": [ 166 | 0.1, 167 | 0.2, 168 | 0.3 169 | ] 170 | }, 171 | 172 | "horizontal_position_controller_4dof": { 173 | "gain": 1.0, 174 | "CT_or_DT": "CT", 175 | "TF_or_PID": "PID", 176 | "kp":0.85, 177 | "ki":0.0, 178 | "kd":0.0, 179 | "crossover_freq_rad_per_sec": 6.283, 180 | "numerator": [ 181 | 0.1, 182 | 0.2, 183 | 0.3 184 | ], 185 | "denominator": [ 186 | 0.1, 187 | 0.2, 188 | 0.3 189 | ] 190 | }, 191 | 192 | "horizontal_position_controller_6dof": { 193 | "gain": 1.0, 194 | "CT_or_DT": "CT", 195 | "TF_or_PID": "PID", 196 | "kp":0.85, 197 | "ki":0.0, 198 | "kd":0.0, 199 | "crossover_freq_rad_per_sec": 6.283, 200 | "numerator": [ 201 | 0.1, 202 | 0.2, 203 | 0.3 204 | ], 205 | "denominator": [ 206 | 0.1, 207 | 0.2, 208 | 0.3 209 | ] 210 | }, 211 | 212 | "max_XY_velocity": 1.0, 213 | "max_Z_velocity": 1.0 214 | } -------------------------------------------------------------------------------- /settings/pgaskell_settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "QUADCOPTER PGASKELL", 3 | 4 | "warnings_en": true, 5 | 6 | "layout": "LAYOUT_4X", 7 | "thrust_map": "LINEAR_MAP", 8 | "orientation": "ORIENTATION_X_FORWARD", 9 | "v_nominal": 11.1, 10 | 11 | "enable_magnetometer": false, 12 | 13 | "num_dsm_modes": 3, 14 | "flight_mode_1": "DIRECT_THROTTLE_4DOF", 15 | "flight_mode_2": "TEST_BENCH_4DOF", 16 | "flight_mode_3": "ALT_HOLD_4DOF", 17 | 18 | "dsm_thr_ch": 1, 19 | "dsm_thr_pol": -1, 20 | "dsm_roll_ch": 2, 21 | "dsm_roll_pol": -1, 22 | "dsm_pitch_ch": 3, 23 | "dsm_pitch_pol": -1, 24 | "dsm_yaw_ch": 4, 25 | "dsm_yaw_pol": -1, 26 | "dsm_mode_ch": 5, 27 | "dsm_mode_pol": 1, 28 | "dsm_kill_mode": "DSM_KILL_DEDICATED_SWITCH", 29 | "dsm_kill_ch": 6, 30 | "dsm_kill_pol": 1, 31 | 32 | "printf_arm": true, 33 | "printf_altitude": true, 34 | "printf_position": true, 35 | "printf_rpy": true, 36 | "printf_sticks": true, 37 | "printf_setpoint": true, 38 | "printf_u": true, 39 | "printf_motors": true, 40 | "printf_mode": true, 41 | 42 | "enable_logging": true, 43 | "log_sensors": true, 44 | "log_state": true, 45 | "log_setpoint": true, 46 | "log_control_u": true, 47 | "log_motor_signals": true, 48 | 49 | "dest_ip": "192.168.8.1", 50 | "my_sys_id": 1, 51 | "mav_port": 14551, 52 | 53 | "roll_controller": { 54 | "gain": 1.0, 55 | "CT_or_DT": "CT", 56 | "TF_or_PID": "PID", 57 | "kp":0.3, 58 | "ki":0.01, 59 | "kd":0.1, 60 | "crossover_freq_rad_per_sec": 62.83, 61 | "numerator": [ 62 | 0.1, 63 | 0.2, 64 | 0.3 65 | ], 66 | "denominator": [ 67 | 0.1, 68 | 0.2, 69 | 0.3 70 | ] 71 | }, 72 | 73 | "pitch_controller": { 74 | "gain": 1.0, 75 | "CT_or_DT": "CT", 76 | "TF_or_PID": "PID", 77 | "kp":0.3, 78 | "ki":0.01, 79 | "kd":0.1, 80 | "crossover_freq_rad_per_sec": 62.83, 81 | "numerator": [ 82 | 0.1, 83 | 0.2, 84 | 0.3 85 | ], 86 | "denominator": [ 87 | 0.1, 88 | 0.2, 89 | 0.3 90 | ] 91 | }, 92 | 93 | "yaw_controller": { 94 | "gain": 1.0, 95 | "CT_or_DT": "CT", 96 | "TF_or_PID": "PID", 97 | "kp":0.3, 98 | "ki":0.01, 99 | "kd":0.1, 100 | "crossover_freq_rad_per_sec": 31.41, 101 | "numerator": [ 102 | 0.1, 103 | 0.2, 104 | 0.3 105 | ], 106 | "denominator": [ 107 | 0.1, 108 | 0.2, 109 | 0.3 110 | ] 111 | }, 112 | 113 | "altitude_controller": { 114 | "gain": 1.0, 115 | "CT_or_DT": "CT", 116 | "TF_or_PID": "PID", 117 | "kp":0.85, 118 | "ki":0.0, 119 | "kd":0.0, 120 | "crossover_freq_rad_per_sec": 6.283, 121 | "numerator": [ 122 | 0.1, 123 | 0.2, 124 | 0.3 125 | ], 126 | "denominator": [ 127 | 0.1, 128 | 0.2, 129 | 0.3 130 | ] 131 | }, 132 | 133 | "horiz_vel_ctrl_4dof": { 134 | "gain": 1.0, 135 | "CT_or_DT": "CT", 136 | "TF_or_PID": "PID", 137 | "kp":0.85, 138 | "ki":0.0, 139 | "kd":0.0, 140 | "crossover_freq_rad_per_sec": 6.283, 141 | "numerator": [ 142 | 0.1, 143 | 0.2, 144 | 0.3 145 | ], 146 | "denominator": [ 147 | 0.1, 148 | 0.2, 149 | 0.3 150 | ] 151 | }, 152 | 153 | "horiz_vel_ctrl_6dof": { 154 | "gain": 1.0, 155 | "CT_or_DT": "CT", 156 | "TF_or_PID": "PID", 157 | "kp":0.85, 158 | "ki":0.0, 159 | "kd":0.0, 160 | "crossover_freq_rad_per_sec": 6.283, 161 | "numerator": [ 162 | 0.1, 163 | 0.2, 164 | 0.3 165 | ], 166 | "denominator": [ 167 | 0.1, 168 | 0.2, 169 | 0.3 170 | ] 171 | }, 172 | 173 | "horiz_pos_ctrl_4dof": { 174 | "gain": 1.0, 175 | "CT_or_DT": "CT", 176 | "TF_or_PID": "PID", 177 | "kp":0.85, 178 | "ki":0.0, 179 | "kd":0.0, 180 | "crossover_freq_rad_per_sec": 6.283, 181 | "numerator": [ 182 | 0.1, 183 | 0.2, 184 | 0.3 185 | ], 186 | "denominator": [ 187 | 0.1, 188 | 0.2, 189 | 0.3 190 | ] 191 | }, 192 | 193 | "horiz_pos_ctrl_6dof": { 194 | "gain": 1.0, 195 | "CT_or_DT": "CT", 196 | "TF_or_PID": "PID", 197 | "kp":0.85, 198 | "ki":0.0, 199 | "kd":0.0, 200 | "crossover_freq_rad_per_sec": 6.283, 201 | "numerator": [ 202 | 0.1, 203 | 0.2, 204 | 0.3 205 | ], 206 | "denominator": [ 207 | 0.1, 208 | 0.2, 209 | 0.3 210 | ] 211 | }, 212 | 213 | "max_XY_velocity": 1.0, 214 | "max_Z_velocity": 1.0 215 | } -------------------------------------------------------------------------------- /src/feedback.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feedback.c 3 | * 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #define TWO_PI (M_PI * 2.0) 28 | 29 | feedback_state_t fstate; // extern variable in feedback.h 30 | 31 | // keep original controller gains for scaling later 32 | static double D_roll_gain_orig, D_pitch_gain_orig, D_yaw_gain_orig, D_Z_gain_orig; 33 | 34 | // filters 35 | static rc_filter_t D_roll = RC_FILTER_INITIALIZER; 36 | static rc_filter_t D_pitch = RC_FILTER_INITIALIZER; 37 | static rc_filter_t D_yaw = RC_FILTER_INITIALIZER; 38 | static rc_filter_t D_Z = RC_FILTER_INITIALIZER; 39 | static rc_filter_t D_Xdot_4 = RC_FILTER_INITIALIZER; 40 | static rc_filter_t D_Xdot_6 = RC_FILTER_INITIALIZER; 41 | static rc_filter_t D_X_4 = RC_FILTER_INITIALIZER; 42 | static rc_filter_t D_X_6 = RC_FILTER_INITIALIZER; 43 | static rc_filter_t D_Ydot_4 = RC_FILTER_INITIALIZER; 44 | static rc_filter_t D_Ydot_6 = RC_FILTER_INITIALIZER; 45 | static rc_filter_t D_Y_4 = RC_FILTER_INITIALIZER; 46 | static rc_filter_t D_Y_6 = RC_FILTER_INITIALIZER; 47 | 48 | static int __send_motor_stop_pulse(void) 49 | { 50 | int i; 51 | if (settings.num_rotors > 8) 52 | { 53 | printf("ERROR: set_motors_to_idle: too many rotors\n"); 54 | return -1; 55 | } 56 | for (i = 0; i < settings.num_rotors; i++) 57 | { 58 | fstate.m[i] = -0.1; 59 | rc_servo_send_esc_pulse_normalized(i + 1, -0.1); 60 | } 61 | return 0; 62 | } 63 | 64 | static void __rpy_init(void) 65 | { 66 | // get controllers from settings 67 | 68 | rc_filter_duplicate(&D_roll, settings.roll_controller); 69 | rc_filter_duplicate(&D_pitch, settings.pitch_controller); 70 | rc_filter_duplicate(&D_yaw, settings.yaw_controller); 71 | 72 | #ifdef DEBUG 73 | printf("ROLL CONTROLLER:\n"); 74 | rc_filter_print(D_roll); 75 | printf("PITCH CONTROLLER:\n"); 76 | rc_filter_print(D_pitch); 77 | printf("YAW CONTROLLER:\n"); 78 | rc_filter_print(D_yaw); 79 | #endif 80 | 81 | // save original gains as we will scale these by battery voltage later 82 | D_roll_gain_orig = D_roll.gain; 83 | D_pitch_gain_orig = D_pitch.gain; 84 | D_yaw_gain_orig = D_yaw.gain; 85 | 86 | // enable saturation. these limits will be changed late but we need to 87 | // enable now so that soft start can also be enabled 88 | rc_filter_enable_saturation(&D_roll, -MAX_ROLL_COMPONENT, MAX_ROLL_COMPONENT); 89 | rc_filter_enable_saturation(&D_pitch, -MAX_PITCH_COMPONENT, MAX_PITCH_COMPONENT); 90 | rc_filter_enable_saturation(&D_yaw, -MAX_YAW_COMPONENT, MAX_YAW_COMPONENT); 91 | // enable soft start 92 | rc_filter_enable_soft_start(&D_roll, SOFT_START_SECONDS); 93 | rc_filter_enable_soft_start(&D_pitch, SOFT_START_SECONDS); 94 | rc_filter_enable_soft_start(&D_yaw, SOFT_START_SECONDS); 95 | } 96 | 97 | int feedback_disarm(void) 98 | { 99 | fstate.arm_state = DISARMED; 100 | // set LEDs 101 | rc_led_set(RC_LED_RED, 1); 102 | rc_led_set(RC_LED_GREEN, 0); 103 | return 0; 104 | } 105 | 106 | int feedback_arm(void) 107 | { 108 | if (fstate.arm_state == ARMED) 109 | { 110 | printf("WARNING: trying to arm when controller is already armed\n"); 111 | return -1; 112 | } 113 | // start a new log file every time controller is armed, this may take some 114 | // time so do it before touching anything else 115 | if (settings.enable_logging) log_manager_init(); 116 | // get the current time 117 | fstate.arm_time_ns = rc_nanos_since_boot(); 118 | // reset the index 119 | fstate.loop_index = 0; 120 | // when swapping from direct throttle to altitude control, the altitude 121 | // controller needs to know the last throttle input for smooth transition 122 | // TODO: Reinitialize altitude bias 123 | // last_en_alt_ctrl = 0; 124 | // last_usr_thr = MIN_Z_COMPONENT; 125 | // yaw estimator can be zero'd too 126 | // TODO: Reinitialize yaw estimate 127 | // num_yaw_spins = 0; 128 | // last_yaw = -mpu_data.fused_TaitBryan[TB_YAW_Z]; // minus because NED coordinates 129 | // zero out all filters 130 | rc_filter_reset(&D_roll); 131 | rc_filter_reset(&D_pitch); 132 | rc_filter_reset(&D_yaw); 133 | rc_filter_reset(&D_Z); 134 | 135 | // prefill filters with current error 136 | rc_filter_prefill_inputs(&D_roll, -state_estimate.roll); 137 | rc_filter_prefill_inputs(&D_pitch, -state_estimate.pitch); 138 | // set LEDs 139 | rc_led_set(RC_LED_RED, 0); 140 | rc_led_set(RC_LED_GREEN, 1); 141 | // last thing is to flag as armed 142 | fstate.arm_state = ARMED; 143 | return 0; 144 | } 145 | 146 | int feedback_init(void) 147 | { 148 | double tmp; 149 | 150 | __rpy_init(); 151 | 152 | rc_filter_duplicate(&D_Z, settings.altitude_controller); 153 | rc_filter_duplicate(&D_Xdot_4, settings.horiz_vel_ctrl_4dof); 154 | rc_filter_duplicate(&D_Xdot_6, settings.horiz_vel_ctrl_6dof); 155 | rc_filter_duplicate(&D_X_4, settings.horiz_pos_ctrl_4dof); 156 | rc_filter_duplicate(&D_X_6, settings.horiz_pos_ctrl_6dof); 157 | rc_filter_duplicate(&D_Ydot_4, settings.horiz_vel_ctrl_4dof); 158 | rc_filter_duplicate(&D_Ydot_6, settings.horiz_vel_ctrl_6dof); 159 | rc_filter_duplicate(&D_Y_4, settings.horiz_pos_ctrl_4dof); 160 | rc_filter_duplicate(&D_Y_6, settings.horiz_pos_ctrl_6dof); 161 | 162 | #ifdef DEBUG 163 | printf("ALTITUDE CONTROLLER:\n"); 164 | rc_filter_print(D_Z); 165 | #endif 166 | 167 | D_Z_gain_orig = D_Z.gain; 168 | 169 | rc_filter_enable_saturation(&D_Z, -1.0, 1.0); 170 | rc_filter_enable_soft_start(&D_Z, SOFT_START_SECONDS); 171 | // make sure everything is disarmed them start the ISR 172 | feedback_disarm(); 173 | fstate.initialized = 1; 174 | 175 | return 0; 176 | } 177 | 178 | int feedback_march(void) 179 | { 180 | int i; 181 | double tmp, min, max; 182 | double u[6], mot[8]; 183 | log_entry_t new_log; 184 | static int last_en_Z_ctrl = 0; 185 | 186 | // Disarm if rc_state is somehow paused without disarming the controller. 187 | // This shouldn't happen if other threads are working properly. 188 | if (rc_get_state() != RUNNING && fstate.arm_state == ARMED) 189 | { 190 | feedback_disarm(); 191 | } 192 | 193 | // check for a tipover 194 | if (fabs(state_estimate.roll) > TIP_ANGLE || fabs(state_estimate.pitch) > TIP_ANGLE) 195 | { 196 | feedback_disarm(); 197 | printf("\n TIPOVER DETECTED \n"); 198 | } 199 | 200 | // if not running or not armed, keep the motors in an idle state 201 | if (rc_get_state() != RUNNING || fstate.arm_state == DISARMED) 202 | { 203 | __send_motor_stop_pulse(); 204 | return 0; 205 | } 206 | 207 | // We are about to start marching the individual SISO controllers forward. 208 | // Start by zeroing out the motors signals then add from there. 209 | for (i = 0; i < 8; i++) mot[i] = 0.0; 210 | for (i = 0; i < 6; i++) u[i] = 0.0; 211 | 212 | /*************************************************************************** 213 | * Throttle/Altitude Controller 214 | * 215 | * If transitioning from direct throttle to altitude control, prefill the 216 | * filter with current throttle input to make smooth transition. This is also 217 | * true if taking off for the first time in altitude mode as arm_controller 218 | * sets up last_en_Z_ctrl and last_usr_thr every time controller arms 219 | ***************************************************************************/ 220 | // run altitude controller if enabled 221 | // this needs work... 222 | // we need to: 223 | // find hover thrust and correct from there 224 | // this code does not work a.t.m. 225 | if (setpoint.en_Z_ctrl) 226 | { 227 | if (last_en_Z_ctrl == 0) 228 | { 229 | setpoint.Z = state_estimate.alt_bmp; // set altitude setpoint to current altitude 230 | rc_filter_reset(&D_Z); 231 | tmp = -setpoint.Z_throttle / (cos(state_estimate.roll) * cos(state_estimate.pitch)); 232 | rc_filter_prefill_outputs(&D_Z, tmp); 233 | last_en_Z_ctrl = 1; 234 | } 235 | D_Z.gain = D_Z_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; 236 | tmp = rc_filter_march( 237 | &D_Z, -setpoint.Z + state_estimate.alt_bmp); // altitude is positive but +Z is down 238 | rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); 239 | u[VEC_Z] = tmp / cos(state_estimate.roll) * cos(state_estimate.pitch); 240 | mix_add_input(u[VEC_Z], VEC_Z, mot); 241 | last_en_Z_ctrl = 1; 242 | } 243 | // else use direct throttle 244 | else 245 | { 246 | // compensate for tilt 247 | tmp = setpoint.Z_throttle / (cos(state_estimate.roll) * cos(state_estimate.pitch)); 248 | // printf("throttle: %f\n",tmp); 249 | rc_saturate_double(&tmp, MIN_THRUST_COMPONENT, MAX_THRUST_COMPONENT); 250 | u[VEC_Z] = tmp; 251 | mix_add_input(u[VEC_Z], VEC_Z, mot); 252 | } 253 | 254 | /*************************************************************************** 255 | * Roll Pitch Yaw controllers, only run if enabled 256 | ***************************************************************************/ 257 | if (setpoint.en_rpy_ctrl) 258 | { 259 | // Roll 260 | mix_check_saturation(VEC_ROLL, mot, &min, &max); 261 | if (max > MAX_ROLL_COMPONENT) max = MAX_ROLL_COMPONENT; 262 | if (min < -MAX_ROLL_COMPONENT) min = -MAX_ROLL_COMPONENT; 263 | rc_filter_enable_saturation(&D_roll, min, max); 264 | D_roll.gain = D_roll_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; 265 | u[VEC_ROLL] = rc_filter_march(&D_roll, setpoint.roll - state_estimate.roll); 266 | mix_add_input(u[VEC_ROLL], VEC_ROLL, mot); 267 | 268 | // pitch 269 | mix_check_saturation(VEC_PITCH, mot, &min, &max); 270 | if (max > MAX_PITCH_COMPONENT) max = MAX_PITCH_COMPONENT; 271 | if (min < -MAX_PITCH_COMPONENT) min = -MAX_PITCH_COMPONENT; 272 | rc_filter_enable_saturation(&D_pitch, min, max); 273 | D_pitch.gain = D_pitch_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; 274 | u[VEC_PITCH] = rc_filter_march(&D_pitch, setpoint.pitch - state_estimate.pitch); 275 | mix_add_input(u[VEC_PITCH], VEC_PITCH, mot); 276 | 277 | // Yaw 278 | // if throttle stick is down (waiting to take off) keep yaw setpoint at 279 | // current heading, otherwide update by yaw rate 280 | mix_check_saturation(VEC_YAW, mot, &min, &max); 281 | if (max > MAX_YAW_COMPONENT) max = MAX_YAW_COMPONENT; 282 | if (min < -MAX_YAW_COMPONENT) min = -MAX_YAW_COMPONENT; 283 | rc_filter_enable_saturation(&D_yaw, min, max); 284 | D_yaw.gain = D_yaw_gain_orig * settings.v_nominal / state_estimate.v_batt_lp; 285 | u[VEC_YAW] = rc_filter_march(&D_yaw, setpoint.yaw - state_estimate.yaw); 286 | mix_add_input(u[VEC_YAW], VEC_YAW, mot); 287 | } 288 | // otherwise direct throttle to roll pitch yaw 289 | else 290 | { 291 | // roll 292 | mix_check_saturation(VEC_ROLL, mot, &min, &max); 293 | if (max > MAX_ROLL_COMPONENT) max = MAX_ROLL_COMPONENT; 294 | if (min < -MAX_ROLL_COMPONENT) min = -MAX_ROLL_COMPONENT; 295 | u[VEC_ROLL] = setpoint.roll_throttle; 296 | rc_saturate_double(&u[VEC_ROLL], min, max); 297 | mix_add_input(u[VEC_ROLL], VEC_ROLL, mot); 298 | 299 | // pitch 300 | mix_check_saturation(VEC_PITCH, mot, &min, &max); 301 | if (max > MAX_PITCH_COMPONENT) max = MAX_PITCH_COMPONENT; 302 | if (min < -MAX_PITCH_COMPONENT) min = -MAX_PITCH_COMPONENT; 303 | u[VEC_PITCH] = setpoint.pitch_throttle; 304 | rc_saturate_double(&u[VEC_PITCH], min, max); 305 | mix_add_input(u[VEC_PITCH], VEC_PITCH, mot); 306 | 307 | // YAW 308 | mix_check_saturation(VEC_YAW, mot, &min, &max); 309 | if (max > MAX_YAW_COMPONENT) max = MAX_YAW_COMPONENT; 310 | if (min < -MAX_YAW_COMPONENT) min = -MAX_YAW_COMPONENT; 311 | u[VEC_YAW] = setpoint.yaw_throttle; 312 | rc_saturate_double(&u[VEC_YAW], min, max); 313 | mix_add_input(u[VEC_YAW], VEC_YAW, mot); 314 | } 315 | 316 | // for 6dof systems, add X and Y 317 | if (setpoint.en_6dof) 318 | { 319 | // X 320 | mix_check_saturation(VEC_X, mot, &min, &max); 321 | if (max > MAX_X_COMPONENT) max = MAX_X_COMPONENT; 322 | if (min < -MAX_X_COMPONENT) min = -MAX_X_COMPONENT; 323 | u[VEC_X] = setpoint.X_throttle; 324 | rc_saturate_double(&u[VEC_X], min, max); 325 | mix_add_input(u[VEC_X], VEC_X, mot); 326 | 327 | // Y 328 | mix_check_saturation(VEC_Y, mot, &min, &max); 329 | if (max > MAX_Y_COMPONENT) max = MAX_Y_COMPONENT; 330 | if (min < -MAX_Y_COMPONENT) min = -MAX_Y_COMPONENT; 331 | u[VEC_Y] = setpoint.Y_throttle; 332 | rc_saturate_double(&u[VEC_Y], min, max); 333 | mix_add_input(u[VEC_Y], VEC_Y, mot); 334 | } 335 | 336 | /*************************************************************************** 337 | * Send ESC motor signals immediately at the end of the control loop 338 | ***************************************************************************/ 339 | for (i = 0; i < settings.num_rotors; i++) 340 | { 341 | rc_saturate_double(&mot[i], 0.0, 1.0); 342 | fstate.m[i] = map_motor_signal(mot[i]); 343 | 344 | // NO NO NO this undoes all the fancy mixing-based saturation 345 | // done above, idle should be done with MAX_THRUST_COMPONENT instead 346 | // rc_saturate_double(&fstate.m[i], MOTOR_IDLE_CMD, 1.0); 347 | 348 | // final saturation just to take care of possible rounding errors 349 | // this should not change the values and is probably excessive 350 | rc_saturate_double(&fstate.m[i], 0.0, 1.0); 351 | 352 | // finally send pulses! 353 | rc_servo_send_esc_pulse_normalized(i + 1, fstate.m[i]); 354 | } 355 | 356 | /*************************************************************************** 357 | * Final cleanup, timing, and indexing 358 | ***************************************************************************/ 359 | // Load control inputs into cstate for viewing by outside threads 360 | for (i = 0; i < 6; i++) fstate.u[i] = u[i]; 361 | // keep track of loops since arming 362 | fstate.loop_index++; 363 | // log us since arming, mostly for the log 364 | fstate.last_step_ns = rc_nanos_since_boot(); 365 | 366 | return 0; 367 | } 368 | 369 | int feedback_cleanup(void) 370 | { 371 | __send_motor_stop_pulse(); 372 | return 0; 373 | } -------------------------------------------------------------------------------- /src/input_manager.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file input_manager.c 3 | */ 4 | 5 | #include 6 | #include // for fabs 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | user_input_t user_input; // extern variable in input_manager.h 23 | 24 | static pthread_t input_manager_thread; 25 | static arm_state_t kill_switch = DISARMED; // raw kill switch on the radio 26 | 27 | /** 28 | * float apply_deadzone(float in, float zone) 29 | * 30 | * Applies a dead zone to an input stick in. in is supposed to range from -1 to 1 31 | * the dead zone is centered around 0. zone specifies the distance from 0 the 32 | * zone extends. 33 | **/ 34 | static double __deadzone(double in, double zone) 35 | { 36 | if (zone <= 0.0) return in; 37 | if (fabs(in) <= zone) return 0.0; 38 | if (in > 0.0) 39 | return ((in - zone) / (1.0 - zone)); 40 | else 41 | return ((in + zone) / (1.0 - zone)); 42 | } 43 | 44 | /** 45 | * @brief blocking function that returns after arming sequence is complete 46 | * 47 | * @return 0 if successful or already armed, -1 if exiting or problem 48 | */ 49 | static int __wait_for_arming_sequence() 50 | { 51 | // already armed, just return. Should never do this in normal operation though 52 | if (user_input.requested_arm_mode == ARMED) return 0; 53 | 54 | ARM_SEQUENCE_START: 55 | // wait for feedback controller to have started 56 | while (fstate.initialized == 0) 57 | { 58 | rc_usleep(100000); 59 | if (rc_get_state() == EXITING) return 0; 60 | } 61 | // wait for level 62 | while (fabs(state_estimate.roll) > ARM_TIP_THRESHOLD || 63 | fabs(state_estimate.pitch) > ARM_TIP_THRESHOLD) 64 | { 65 | rc_usleep(100000); 66 | if (rc_get_state() == EXITING) return 0; 67 | } 68 | // wait for kill switch to be switched to ARMED 69 | while (kill_switch == DISARMED) 70 | { 71 | rc_usleep(100000); 72 | if (rc_get_state() == EXITING) return 0; 73 | } 74 | // wait for throttle up 75 | while (rc_dsm_ch_normalized(settings.dsm_thr_ch) * settings.dsm_thr_pol < 0.9) 76 | { 77 | rc_usleep(100000); 78 | if (rc_get_state() == EXITING) return 0; 79 | if (kill_switch == DISARMED) goto ARM_SEQUENCE_START; 80 | } 81 | // wait for throttle down 82 | while (rc_dsm_ch_normalized(settings.dsm_thr_ch) * settings.dsm_thr_pol > -0.9) 83 | { 84 | rc_usleep(100000); 85 | if (rc_get_state() == EXITING) return 0; 86 | if (kill_switch == DISARMED) goto ARM_SEQUENCE_START; 87 | } 88 | 89 | // final check of kill switch and level before arming 90 | if (kill_switch == DISARMED) goto ARM_SEQUENCE_START; 91 | if (fabs(state_estimate.roll) > ARM_TIP_THRESHOLD || 92 | fabs(state_estimate.pitch) > ARM_TIP_THRESHOLD) 93 | { 94 | goto ARM_SEQUENCE_START; 95 | } 96 | return 0; 97 | } 98 | 99 | void new_dsm_data_callback() 100 | { 101 | double new_thr, new_roll, new_pitch, new_yaw, new_mode, new_kill; 102 | 103 | // Read normalized (+-1) inputs from RC radio stick and multiply by 104 | // polarity setting so positive stick means positive setpoint 105 | new_thr = rc_dsm_ch_normalized(settings.dsm_thr_ch) * settings.dsm_thr_pol; 106 | new_roll = rc_dsm_ch_normalized(settings.dsm_roll_ch) * settings.dsm_roll_pol; 107 | new_pitch = rc_dsm_ch_normalized(settings.dsm_pitch_ch) * settings.dsm_pitch_pol; 108 | new_yaw = 109 | __deadzone(rc_dsm_ch_normalized(settings.dsm_yaw_ch) * settings.dsm_yaw_pol, YAW_DEADZONE); 110 | new_mode = rc_dsm_ch_normalized(settings.dsm_mode_ch) * settings.dsm_mode_pol; 111 | 112 | // kill mode behaviors 113 | switch (settings.dsm_kill_mode) 114 | { 115 | case DSM_KILL_DEDICATED_SWITCH: 116 | new_kill = rc_dsm_ch_normalized(settings.dsm_kill_ch) * settings.dsm_kill_pol; 117 | // determine the kill state 118 | if (new_kill <= 0.1) 119 | { 120 | kill_switch = DISARMED; 121 | user_input.requested_arm_mode = DISARMED; 122 | } 123 | else 124 | { 125 | kill_switch = ARMED; 126 | } 127 | break; 128 | 129 | case DSM_KILL_NEGATIVE_THROTTLE: 130 | if (new_thr <= -1.1) 131 | { 132 | kill_switch = DISARMED; 133 | user_input.requested_arm_mode = DISARMED; 134 | } 135 | else 136 | kill_switch = ARMED; 137 | break; 138 | 139 | default: 140 | fprintf(stderr, "ERROR in input manager, unhandled settings.dsm_kill_mode\n"); 141 | return; 142 | } 143 | 144 | // saturate the sticks to avoid possible erratic behavior 145 | // throttle can drop below -1 so extend the range for thr 146 | rc_saturate_double(&new_thr, -1.0, 1.0); 147 | rc_saturate_double(&new_roll, -1.0, 1.0); 148 | rc_saturate_double(&new_pitch, -1.0, 1.0); 149 | rc_saturate_double(&new_yaw, -1.0, 1.0); 150 | 151 | // pick flight mode 152 | switch (settings.num_dsm_modes) 153 | { 154 | case 1: 155 | user_input.flight_mode = settings.flight_mode_1; 156 | break; 157 | case 2: 158 | // switch will either range from -1 to 1 or 0 to 1. 159 | // in either case it's safe to use +0.5 as the cutoff 160 | if (new_mode > 0.5) 161 | user_input.flight_mode = settings.flight_mode_2; 162 | else 163 | user_input.flight_mode = settings.flight_mode_1; 164 | break; 165 | case 3: 166 | // 3-position switch will have the positions -1, 0, 1 when 167 | // calibrated correctly. checking +- 0.5 is a safe cutoff 168 | if (new_mode > 0.5) 169 | user_input.flight_mode = settings.flight_mode_3; 170 | else if (new_mode < -0.5) 171 | user_input.flight_mode = settings.flight_mode_1; 172 | else 173 | user_input.flight_mode = settings.flight_mode_2; 174 | break; 175 | default: 176 | fprintf(stderr, "ERROR, in input_manager, num_dsm_modes must be 1,2 or 3\n"); 177 | fprintf(stderr, "selecting flight mode 1\n"); 178 | user_input.flight_mode = settings.flight_mode_1; 179 | break; 180 | } 181 | 182 | // fill in sticks 183 | if (user_input.requested_arm_mode == ARMED) 184 | { 185 | user_input.thr_stick = new_thr; 186 | user_input.roll_stick = new_roll; 187 | user_input.pitch_stick = new_pitch; 188 | user_input.yaw_stick = new_yaw; 189 | user_input.requested_arm_mode = kill_switch; 190 | } 191 | else 192 | { 193 | // during arming sequence keep sticks zeroed 194 | user_input.thr_stick = 0.0; 195 | user_input.roll_stick = 0.0; 196 | user_input.pitch_stick = 0.0; 197 | user_input.yaw_stick = 0.0; 198 | } 199 | 200 | if (user_input.input_active == 0) 201 | { 202 | user_input.input_active = 1; // flag that connection has come back online 203 | printf("DSM CONNECTION ESTABLISHED\n"); 204 | } 205 | return; 206 | } 207 | 208 | void dsm_disconnect_callback(void) 209 | { 210 | user_input.thr_stick = 0.0; 211 | user_input.roll_stick = 0.0; 212 | user_input.pitch_stick = 0.0; 213 | user_input.yaw_stick = 0.0; 214 | user_input.input_active = 0; 215 | kill_switch = DISARMED; 216 | user_input.requested_arm_mode = DISARMED; 217 | fprintf(stderr, "LOST DSM CONNECTION\n"); 218 | } 219 | 220 | void* input_manager(void* ptr) 221 | { 222 | user_input.initialized = 1; 223 | // wait for first packet 224 | while (rc_get_state() != EXITING) 225 | { 226 | if (user_input.input_active) break; 227 | rc_usleep(1000000 / INPUT_MANAGER_HZ); 228 | } 229 | 230 | // not much to do since the DSM callbacks to most of it. Later some 231 | // logic to handle other inputs such as mavlink/bluetooth/wifi 232 | while (rc_get_state() != EXITING) 233 | { 234 | // if the core got disarmed, wait for arming sequence 235 | if (user_input.requested_arm_mode == DISARMED) 236 | { 237 | __wait_for_arming_sequence(); 238 | // user may have pressed the pause button or shut down while waiting 239 | // check before continuing 240 | if (rc_get_state() != RUNNING) 241 | continue; 242 | else 243 | { 244 | user_input.requested_arm_mode = ARMED; 245 | // printf("\n\nDSM ARM REQUEST\n\n"); 246 | } 247 | } 248 | // wait 249 | rc_usleep(1000000 / INPUT_MANAGER_HZ); 250 | } 251 | return NULL; 252 | } 253 | 254 | int input_manager_init() 255 | { 256 | user_input.initialized = 0; 257 | int i; 258 | // start dsm hardware 259 | if (rc_dsm_init() == -1) 260 | { 261 | fprintf(stderr, "ERROR in input_manager_init, failed to initialize dsm\n"); 262 | return -1; 263 | } 264 | rc_dsm_set_disconnect_callback(dsm_disconnect_callback); 265 | rc_dsm_set_callback(new_dsm_data_callback); 266 | // start thread 267 | if (rc_pthread_create( 268 | &input_manager_thread, &input_manager, NULL, SCHED_FIFO, INPUT_MANAGER_PRI) == -1) 269 | { 270 | fprintf(stderr, "ERROR in input_manager_init, failed to start thread\n"); 271 | return -1; 272 | } 273 | // wait for thread to start 274 | for (i = 0; i < 50; i++) 275 | { 276 | if (user_input.initialized) return 0; 277 | rc_usleep(50000); 278 | } 279 | fprintf(stderr, "ERROR in input_manager_init, timeout waiting for thread to start\n"); 280 | return -1; 281 | } 282 | 283 | int input_manager_cleanup() 284 | { 285 | if (user_input.initialized == 0) 286 | { 287 | fprintf(stderr, "WARNING in input_manager_cleanup, was never initialized\n"); 288 | return -1; 289 | } 290 | // wait for the thread to exit 291 | if (rc_pthread_timed_join(input_manager_thread, NULL, INPUT_MANAGER_TOUT) == 1) 292 | { 293 | fprintf(stderr, "WARNING: in input_manager_cleanup, thread join timeout\n"); 294 | return -1; 295 | } 296 | // stop dsm 297 | rc_dsm_cleanup(); 298 | return 0; 299 | } 300 | -------------------------------------------------------------------------------- /src/log_manager.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file log_manager.c 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // to allow printf macros for multi-architecture portability 14 | #define __STDC_FORMAT_MACROS 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define MAX_LOG_FILES 500 30 | #define BUF_LEN 50 31 | 32 | static uint64_t num_entries; // number of entries logged so far 33 | static int buffer_pos; // position in current buffer 34 | static int current_buf; // 0 or 1 to indicate which buffer is being filled 35 | static int needs_writing; // flag set to 1 if a buffer is full 36 | static FILE* fd; // file descriptor for the log file 37 | 38 | // array of two buffers so one can fill while writing the other to file 39 | static log_entry_t buffer[2][BUF_LEN]; 40 | 41 | // background thread and running flag 42 | static pthread_t pthread; 43 | static int logging_enabled; // set to 0 to exit the write_thread 44 | 45 | static int __write_header(FILE* fd) 46 | { 47 | // always print loop index 48 | fprintf(fd, "loop_index,last_step_ns"); 49 | 50 | if (settings.log_sensors) 51 | { 52 | fprintf(fd, ",v_batt,alt_bmp_raw,gyro_roll,gyro_pitch,gyro_yaw,accel_X,accel_Y,accel_Z"); 53 | } 54 | 55 | if (settings.log_state) 56 | { 57 | fprintf(fd, ",roll,pitch,yaw,X,Y,Z,Xdot,Ydot,Zdot"); 58 | } 59 | 60 | if (settings.log_setpoint) 61 | { 62 | fprintf(fd, ",sp_roll,sp_pitch,sp_yaw,sp_X,sp_Y,sp_Z,sp_Xdot,sp_Ydot,sp_Zdot"); 63 | } 64 | 65 | if (settings.log_control_u) 66 | { 67 | fprintf(fd, ",u_roll,u_pitch,u_yaw,u_X,u_Y,u_Z"); 68 | } 69 | 70 | if (settings.log_motor_signals && settings.num_rotors == 8) 71 | { 72 | fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6,mot_7,mot_8"); 73 | } 74 | if (settings.log_motor_signals && settings.num_rotors == 6) 75 | { 76 | fprintf(fd, ",mot_1,mot_2,mot_3,mot_4,mot_5,mot_6"); 77 | } 78 | if (settings.log_motor_signals && settings.num_rotors == 4) 79 | { 80 | fprintf(fd, ",mot_1,mot_2,mot_3,mot_4"); 81 | } 82 | 83 | fprintf(fd, "\n"); 84 | return 0; 85 | } 86 | 87 | static int __write_log_entry(FILE* fd, log_entry_t e) 88 | { 89 | // always print loop index 90 | fprintf(fd, "%" PRIu64 ",%" PRIu64, e.loop_index, e.last_step_ns); 91 | 92 | if (settings.log_sensors) 93 | { 94 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.v_batt, e.alt_bmp_raw, 95 | e.gyro_roll, e.gyro_pitch, e.gyro_yaw, e.accel_X, e.accel_Y, e.accel_Z); 96 | } 97 | 98 | if (settings.log_state) 99 | { 100 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.roll, e.pitch, e.yaw, e.X, 101 | e.Y, e.Z, e.Xdot, e.Ydot, e.Zdot); 102 | } 103 | 104 | if (settings.log_setpoint) 105 | { 106 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.sp_roll, e.sp_pitch, 107 | e.sp_yaw, e.sp_X, e.sp_Y, e.sp_Z, e.sp_Xdot, e.sp_Ydot, e.sp_Zdot); 108 | } 109 | 110 | if (settings.log_control_u) 111 | { 112 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.u_roll, e.u_pitch, e.u_yaw, 113 | e.u_X, e.u_Y, e.u_Z); 114 | } 115 | 116 | if (settings.log_motor_signals && settings.num_rotors == 8) 117 | { 118 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4, 119 | e.mot_5, e.mot_6, e.mot_7, e.mot_8); 120 | } 121 | if (settings.log_motor_signals && settings.num_rotors == 6) 122 | { 123 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4, e.mot_5, 124 | e.mot_6); 125 | } 126 | if (settings.log_motor_signals && settings.num_rotors == 4) 127 | { 128 | fprintf(fd, ",%.4F,%.4F,%.4F,%.4F", e.mot_1, e.mot_2, e.mot_3, e.mot_4); 129 | } 130 | 131 | fprintf(fd, "\n"); 132 | return 0; 133 | } 134 | 135 | static void* __log_manager_func(__attribute__((unused)) void* ptr) 136 | { 137 | int i, buf_to_write; 138 | // while logging enabled and not exiting, write full buffers to disk 139 | while (rc_get_state() != EXITING && logging_enabled) 140 | { 141 | if (needs_writing) 142 | { 143 | // buffer to be written is opposite of one currently being filled 144 | if (current_buf == 0) 145 | buf_to_write = 1; 146 | else 147 | buf_to_write = 0; 148 | // write the full buffer to disk; 149 | for (i = 0; i < BUF_LEN; i++) 150 | { 151 | __write_log_entry(fd, buffer[buf_to_write][i]); 152 | } 153 | fflush(fd); 154 | needs_writing = 0; 155 | } 156 | rc_usleep(1000000 / LOG_MANAGER_HZ); 157 | } 158 | 159 | // if program is exiting or logging got disabled, write out the rest of 160 | // the logs that are in the buffer current being filled 161 | // printf("writing out remaining log file\n"); 162 | for (i = 0; i < buffer_pos; i++) 163 | { 164 | __write_log_entry(fd, buffer[current_buf][i]); 165 | } 166 | fflush(fd); 167 | fclose(fd); 168 | 169 | // zero out state 170 | logging_enabled = 0; 171 | num_entries = 0; 172 | buffer_pos = 0; 173 | current_buf = 0; 174 | needs_writing = 0; 175 | return NULL; 176 | } 177 | 178 | int log_manager_init() 179 | { 180 | int i; 181 | char path[100]; 182 | struct stat st = {0}; 183 | 184 | // if the thread if running, stop before starting a new log file 185 | if (logging_enabled) 186 | { 187 | // fprintf(stderr,"ERROR: in start_log_manager, log manager already running.\n"); 188 | // return -1; 189 | log_manager_cleanup(); 190 | } 191 | 192 | // first make sure the directory exists, make it if not 193 | if (stat(LOG_DIR, &st) == -1) 194 | { 195 | mkdir(LOG_DIR, 0755); 196 | } 197 | 198 | // search for existing log files to determine the next number in the series 199 | for (i = 1; i <= MAX_LOG_FILES + 1; i++) 200 | { 201 | memset(&path, 0, sizeof(path)); 202 | sprintf(path, LOG_DIR "%d.csv", i); 203 | // if file exists, move onto the next index 204 | if (stat(path, &st) == 0) 205 | continue; 206 | else 207 | break; 208 | } 209 | // limit number of log files 210 | if (i == MAX_LOG_FILES + 1) 211 | { 212 | fprintf(stderr, "ERROR: log file limit exceeded\n"); 213 | fprintf(stderr, "delete old log files before continuing\n"); 214 | return -1; 215 | } 216 | // create and open new file for writing 217 | fd = fopen(path, "w+"); 218 | if (fd == 0) 219 | { 220 | printf("ERROR: can't open log file for writing\n"); 221 | return -1; 222 | } 223 | 224 | // write header 225 | __write_header(fd); 226 | 227 | // start thread 228 | logging_enabled = 1; 229 | num_entries = 0; 230 | buffer_pos = 0; 231 | current_buf = 0; 232 | needs_writing = 0; 233 | 234 | // start logging thread 235 | if (rc_pthread_create(&pthread, __log_manager_func, NULL, SCHED_FIFO, LOG_MANAGER_PRI) < 0) 236 | { 237 | fprintf(stderr, "ERROR in start_log_manager, failed to start thread\n"); 238 | return -1; 239 | } 240 | rc_usleep(1000); 241 | return 0; 242 | } 243 | 244 | static log_entry_t __construct_new_entry() 245 | { 246 | log_entry_t l; 247 | l.loop_index = fstate.loop_index; 248 | l.last_step_ns = fstate.last_step_ns; 249 | 250 | l.v_batt = state_estimate.v_batt_lp; 251 | l.alt_bmp_raw = state_estimate.alt_bmp_raw; 252 | l.gyro_roll = state_estimate.gyro[0]; 253 | l.gyro_pitch = state_estimate.gyro[1]; 254 | l.gyro_yaw = state_estimate.gyro[2]; 255 | l.accel_X = state_estimate.accel[0]; 256 | l.accel_Y = state_estimate.accel[1]; 257 | l.accel_Z = state_estimate.accel[2]; 258 | 259 | l.roll = state_estimate.tb_imu[0]; 260 | l.pitch = state_estimate.tb_imu[1]; 261 | l.yaw = state_estimate.tb_imu[2]; 262 | l.X = state_estimate.pos_global[0]; 263 | l.Y = state_estimate.pos_global[1]; 264 | l.Z = state_estimate.pos_global[2]; 265 | l.Xdot = state_estimate.vel_global[0]; 266 | l.Ydot = state_estimate.vel_global[1]; 267 | l.Zdot = state_estimate.vel_global[2]; 268 | 269 | l.sp_roll = setpoint.roll; 270 | l.sp_pitch = setpoint.pitch; 271 | l.sp_yaw = setpoint.yaw; 272 | l.sp_X = setpoint.X; 273 | l.sp_Y = setpoint.Y; 274 | l.sp_Z = setpoint.Z; 275 | l.sp_Xdot = setpoint.X_dot; 276 | l.sp_Ydot = setpoint.Y_dot; 277 | l.sp_Zdot = setpoint.Z_dot; 278 | 279 | l.u_roll = fstate.u[VEC_ROLL]; 280 | l.u_pitch = fstate.u[VEC_PITCH]; 281 | l.u_yaw = fstate.u[VEC_YAW]; 282 | l.u_X = fstate.u[VEC_Y]; 283 | l.u_Y = fstate.u[VEC_X]; 284 | l.u_Z = fstate.u[VEC_Z]; 285 | 286 | l.mot_1 = fstate.m[0]; 287 | l.mot_2 = fstate.m[1]; 288 | l.mot_3 = fstate.m[2]; 289 | l.mot_4 = fstate.m[3]; 290 | l.mot_5 = fstate.m[4]; 291 | l.mot_6 = fstate.m[5]; 292 | l.mot_7 = fstate.m[6]; 293 | l.mot_8 = fstate.m[7]; 294 | 295 | return l; 296 | } 297 | 298 | int log_manager_add_new() 299 | { 300 | if (!logging_enabled) 301 | { 302 | fprintf(stderr, "ERROR: trying to log entry while logger isn't running\n"); 303 | return -1; 304 | } 305 | if (needs_writing && buffer_pos >= BUF_LEN) 306 | { 307 | fprintf(stderr, "WARNING: logging buffer full, skipping log entry\n"); 308 | return -1; 309 | } 310 | // add to buffer and increment counters 311 | buffer[current_buf][buffer_pos] = __construct_new_entry(); 312 | buffer_pos++; 313 | num_entries++; 314 | // check if we've filled a buffer 315 | if (buffer_pos >= BUF_LEN) 316 | { 317 | buffer_pos = 0; // reset buffer position to 0 318 | needs_writing = 1; // flag the writer to dump to disk 319 | // swap buffers 320 | if (current_buf == 0) 321 | current_buf = 1; 322 | else 323 | current_buf = 0; 324 | } 325 | return 0; 326 | } 327 | 328 | int log_manager_cleanup() 329 | { 330 | // just return if not logging 331 | if (logging_enabled == 0) return 0; 332 | 333 | // disable logging so the thread can stop and start multiple times 334 | // thread also exits on rc_get_state()==EXITING 335 | logging_enabled = 0; 336 | int ret = rc_pthread_timed_join(pthread, NULL, LOG_MANAGER_TOUT); 337 | if (ret == 1) 338 | fprintf(stderr, "WARNING: log_manager_thread exit timeout\n"); 339 | else if (ret == -1) 340 | fprintf(stderr, "ERROR: failed to join log_manager thread\n"); 341 | return ret; 342 | } 343 | -------------------------------------------------------------------------------- /src/main.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.c 3 | * 4 | * see README.txt for description and use 5 | **/ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include // contains extern settings variable 28 | #include 29 | #include 30 | 31 | #define FAIL(str) \ 32 | fprintf(stderr, str); \ 33 | rc_led_set(RC_LED_GREEN, 0); \ 34 | rc_led_blink(RC_LED_RED, 8.0, 2.0); \ 35 | return -1; 36 | 37 | void print_usage() 38 | { 39 | printf("\n"); 40 | printf(" Options\n"); 41 | printf(" -s {settings file} Specify settings file to use\n"); 42 | printf(" -h Print this help message\n"); 43 | printf("\n"); 44 | printf("Some example settings files are included with the\n"); 45 | printf("source code. You must specify the location of one of these\n"); 46 | printf("files or ideally the location of your own settings file.\n"); 47 | printf("\n"); 48 | } 49 | 50 | /** 51 | * temporary check for dsm calibration until I add this to librobotcontrol 52 | */ 53 | int __rc_dsm_is_calibrated() 54 | { 55 | if (!access("/var/lib/robotcontrol/dsm.cal", F_OK)) 56 | return 1; 57 | else 58 | return 0; 59 | } 60 | 61 | /** 62 | * If the user holds the pause button for 2 seconds, set state to exiting which 63 | * triggers the rest of the program to exit cleanly. 64 | */ 65 | void on_pause_press() 66 | { 67 | int i = 0; 68 | const int quit_check_us = 100000; 69 | const int samples = 2000000 / quit_check_us; 70 | 71 | // toggle betewen paused and running modes 72 | if (rc_get_state() == RUNNING) 73 | { 74 | rc_set_state(PAUSED); 75 | printf("PAUSED\n"); 76 | } 77 | else if (rc_get_state() == PAUSED) 78 | { 79 | rc_set_state(RUNNING); 80 | printf("RUNNING\n"); 81 | } 82 | fflush(stdout); 83 | 84 | // now keep checking to see if the button is still held down 85 | for (i = 0; i < samples; i++) 86 | { 87 | rc_usleep(quit_check_us); 88 | if (rc_button_get_state(RC_BTN_PIN_PAUSE) == RC_BTN_STATE_RELEASED) 89 | { 90 | return; 91 | } 92 | } 93 | printf("long press detected, shutting down\n"); 94 | rc_set_state(EXITING); 95 | return; 96 | } 97 | 98 | /** 99 | * @brief Interrupt service routine for IMU 100 | * 101 | * This is called every time the Invensense IMU has new data 102 | */ 103 | static void __imu_isr(void) 104 | { 105 | // printf("imu interupt...\n"); 106 | setpoint_manager_update(); 107 | state_estimator_march(); 108 | feedback_march(); 109 | if (settings.enable_logging) log_manager_add_new(); 110 | state_estimator_jobs_after_feedback(); 111 | } 112 | 113 | /** 114 | * Initialize the IMU, start all the threads, and wait until something triggers 115 | * a shut down by setting the RC state to EXITING. 116 | * 117 | * @return 0 on success, -1 on failure 118 | */ 119 | int main(int argc, char* argv[]) 120 | { 121 | int c; 122 | char* settings_file_path = NULL; 123 | 124 | // parse arguments 125 | opterr = 0; 126 | while ((c = getopt(argc, argv, "s:h")) != -1) 127 | { 128 | switch (c) 129 | { 130 | // settings file option 131 | case 's': 132 | settings_file_path = optarg; 133 | printf("User specified settings file:\n%s\n", settings_file_path); 134 | break; 135 | 136 | // help mode 137 | case 'h': 138 | print_usage(); 139 | return 0; 140 | 141 | default: 142 | printf("\nInvalid Argument \n"); 143 | print_usage(); 144 | return -1; 145 | } 146 | } 147 | 148 | // settings file option is mandatory 149 | if (settings_file_path == NULL) 150 | { 151 | print_usage(); 152 | return -1; 153 | } 154 | 155 | // first things first, load settings which may be used during startup 156 | if (settings_load_from_file(settings_file_path) < 0) 157 | { 158 | fprintf(stderr, "ERROR: failed to load settings\n"); 159 | return -1; 160 | } 161 | printf("Loaded settings: %s\n", settings.name); 162 | 163 | // before touching hardware, make sure another instance isn't running 164 | // return value -3 means a root process is running and we need more 165 | // privileges to stop it. 166 | if (rc_kill_existing_process(2.0) == -3) return -1; 167 | 168 | // start with both LEDs off 169 | if (rc_led_set(RC_LED_GREEN, 0) == -1) 170 | { 171 | fprintf(stderr, "ERROR in main(), failed to set RC_LED_GREEN\n"); 172 | return -1; 173 | } 174 | if (rc_led_set(RC_LED_RED, 0) == -1) 175 | { 176 | fprintf(stderr, "ERROR in main() failed to set RC_LED_RED\n"); 177 | return -1; 178 | } 179 | 180 | // make sure IMU is calibrated 181 | if (!rc_mpu_is_gyro_calibrated()) 182 | { 183 | FAIL("ERROR, must calibrate gyroscope with rc_calibrate_gyro first\n") 184 | } 185 | if (!rc_mpu_is_accel_calibrated()) 186 | { 187 | FAIL("ERROR, must calibrate accelerometer with rc_calibrate_accel first\n") 188 | } 189 | if (settings.enable_magnetometer && !rc_mpu_is_gyro_calibrated()) 190 | { 191 | FAIL("ERROR, must calibrate magnetometer with rc_calibrate_mag first\n") 192 | } 193 | if (!__rc_dsm_is_calibrated()) 194 | { 195 | FAIL("ERROR, must calibrate DSM with rc_calibrate_dsm first\n") 196 | } 197 | 198 | // turn cpu freq to max for most consistent performance and lowest 199 | // latency servicing the IMU's interrupt service routine 200 | // this also serves as an initial check for root access which is needed 201 | // by the PRU later. PRU root acces might get resolved in the future. 202 | if (rc_cpu_set_governor(RC_GOV_PERFORMANCE) < 0) 203 | { 204 | FAIL("WARNING, can't set CPU governor, need to run as root\n") 205 | } 206 | 207 | // do initialization not involving threads 208 | printf("initializing thrust map\n"); 209 | if (thrust_map_init(settings.thrust_map) < 0) 210 | { 211 | FAIL("ERROR: failed to initialize thrust map\n") 212 | } 213 | printf("initializing mixing matrix\n"); 214 | if (mix_init(settings.layout) < 0) 215 | { 216 | FAIL("ERROR: failed to initialize mixing matrix\n") 217 | } 218 | printf("initializing setpoint_manager\n"); 219 | if (setpoint_manager_init() < 0) 220 | { 221 | FAIL("ERROR: failed to initialize setpoint_manager\n") 222 | } 223 | 224 | // initialize cape hardware, this prints an error itself if unsuccessful 225 | printf("initializing servos\n"); 226 | if (rc_servo_init() == -1) 227 | { 228 | FAIL("ERROR: failed to initialize servos, probably need to run as root\n") 229 | } 230 | printf("initializing adc\n"); 231 | if (rc_adc_init() == -1) 232 | { 233 | FAIL("ERROR: failed to initialize ADC") 234 | } 235 | 236 | // start signal handler so threads can exit cleanly 237 | printf("initializing signal handler\n"); 238 | if (rc_enable_signal_handler() < 0) 239 | { 240 | FAIL("ERROR: failed to complete rc_enable_signal_handler\n") 241 | } 242 | 243 | // start threads 244 | printf("initializing DSM and input_manager\n"); 245 | if (input_manager_init() < 0) 246 | { 247 | FAIL("ERROR: failed to initialize input_manager\n") 248 | } 249 | 250 | // initialize buttons and Assign functions to be called when button 251 | // events occur 252 | if (rc_button_init(RC_BTN_PIN_PAUSE, RC_BTN_POLARITY_NORM_HIGH, RC_BTN_DEBOUNCE_DEFAULT_US)) 253 | { 254 | FAIL("ERROR: failed to init buttons\n") 255 | } 256 | rc_button_set_callbacks(RC_BTN_PIN_PAUSE, on_pause_press, NULL); 257 | 258 | // initialize log_manager if enabled in settings 259 | if (settings.enable_logging) 260 | { 261 | printf("initializing log manager"); 262 | if (log_manager_init() < 0) 263 | { 264 | FAIL("ERROR: failed to initialize log manager\n") 265 | } 266 | } 267 | 268 | // start barometer, must do before starting state estimator 269 | printf("initializing Barometer\n"); 270 | if (rc_bmp_init(BMP_OVERSAMPLE_16, BMP_FILTER_16)) 271 | { 272 | FAIL("ERROR: failed to initialize barometer\n") 273 | } 274 | 275 | // set up state estimator 276 | printf("initializing state_estimator\n"); 277 | if (state_estimator_init() < 0) 278 | { 279 | FAIL("ERROR: failed to init state_estimator") 280 | } 281 | 282 | // set up feedback controller 283 | printf("initializing feedback controller\n"); 284 | if (feedback_init() < 0) 285 | { 286 | FAIL("ERROR: failed to init feedback controller") 287 | } 288 | 289 | // start the IMU 290 | rc_mpu_config_t mpu_conf = rc_mpu_default_config(); 291 | mpu_conf.i2c_bus = I2C_BUS; 292 | mpu_conf.gpio_interrupt_pin_chip = GPIO_INT_PIN_CHIP; 293 | mpu_conf.gpio_interrupt_pin = GPIO_INT_PIN_PIN; 294 | mpu_conf.dmp_sample_rate = FEEDBACK_HZ; 295 | mpu_conf.dmp_fetch_accel_gyro = 1; 296 | // mpu_conf.orient = ORIENTATION_Z_UP; 297 | mpu_conf.dmp_interrupt_sched_policy = SCHED_FIFO; 298 | mpu_conf.dmp_interrupt_priority = IMU_PRIORITY; 299 | 300 | // optionally enbale magnetometer 301 | mpu_conf.enable_magnetometer = settings.enable_magnetometer; 302 | 303 | // now set up the imu for dmp interrupt operation 304 | printf("initializing MPU\n"); 305 | if (rc_mpu_initialize_dmp(&mpu_data, mpu_conf)) 306 | { 307 | fprintf(stderr, "ERROR: failed to start MPU DMP\n"); 308 | return -1; 309 | } 310 | 311 | // final setup 312 | if (rc_make_pid_file() != 0) 313 | { 314 | FAIL("ERROR: failed to make a PID file\n") 315 | } 316 | 317 | // make sure everything is disarmed them start the ISR 318 | feedback_disarm(); 319 | printf("waiting for dmp to settle...\n"); 320 | fflush(stdout); 321 | rc_usleep(3000000); 322 | if (rc_mpu_set_dmp_callback(__imu_isr) != 0) 323 | { 324 | FAIL("ERROR: failed to set dmp callback function\n") 325 | } 326 | 327 | // start printf_thread if running from a terminal 328 | // if it was started as a background process then don't bother 329 | 330 | if (isatty(fileno(stdout))) 331 | { 332 | printf("initializing printf manager\n"); 333 | if (printf_init() < 0) 334 | { 335 | FAIL("ERROR: failed to initialize printf_manager\n") 336 | } 337 | } 338 | 339 | // set state to running and chill until something exits the program 340 | rc_set_state(RUNNING); 341 | while (rc_get_state() != EXITING) 342 | { 343 | usleep(50000); 344 | } 345 | 346 | // some of these, like printf_manager and log_manager, have cleanup 347 | // functions that can be called even if not being used. So just call all 348 | // cleanup functions here. 349 | printf("cleaning up\n"); 350 | rc_mpu_power_off(); 351 | feedback_cleanup(); 352 | input_manager_cleanup(); 353 | setpoint_manager_cleanup(); 354 | printf_cleanup(); 355 | log_manager_cleanup(); 356 | 357 | // turn off red LED and blink green to say shut down was safe 358 | rc_led_set(RC_LED_RED, 0); 359 | rc_led_blink(RC_LED_GREEN, 8.0, 2.0); 360 | return 0; 361 | } 362 | -------------------------------------------------------------------------------- /src/mavlink_manager.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mavlink_manager.c 3 | * 4 | * 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #define LOCALHOST_IP "127.0.0.1" 16 | #define DEFAULT_SYS_ID 1 17 | 18 | static void __callback_func_mocap(void) 19 | { 20 | int i; 21 | mavlink_att_pos_mocap_t data; 22 | 23 | if (rc_mav_get_att_pos_mocap(&data) < 0) 24 | { 25 | fprintf(stderr, "ERROR in mavlink manager, problem fetching att_pos_mocal packet\n"); 26 | return; 27 | } 28 | 29 | // check if position is 0 0 0 which indicates mocap system is alive but 30 | // has lost visual contact on the object 31 | if (fabs(data.x) < 0.0001 && fabs(data.y) < 0.0001 && fabs(data.z) < 0.0001) 32 | { 33 | if (state_estimate.mocap_running == 1) 34 | { 35 | state_estimate.mocap_running = 0; 36 | if (settings.warnings_en) 37 | { 38 | fprintf(stderr, "WARNING, MOCAP LOST VISUAL\n"); 39 | } 40 | } 41 | else 42 | { 43 | state_estimate.is_active = 0; 44 | } 45 | return; 46 | } 47 | 48 | // copy data 49 | for (i = 0; i < 4; i++) state_estimate.quat_mocap[i] = (double)data.q[i]; 50 | // normalize quaternion because we don't trust the mocap system 51 | rc_quaternion_norm_array(state_estimate.quat_mocap); 52 | // calculate tait bryan angles too 53 | rc_quaternion_to_tb_array(state_estimate.quat_mocap, state_estimate.tb_mocap); 54 | // position 55 | state_estimate.pos_mocap[0] = (double)data.x; 56 | state_estimate.pos_mocap[1] = (double)data.y; 57 | state_estimate.pos_mocap[2] = (double)data.z; 58 | // mark timestamp 59 | state_estimate.mocap_timestamp_ns = rc_nanos_since_boot(); 60 | state_estimate.mocap_running = 1; 61 | 62 | return; 63 | } 64 | 65 | int mavlink_manager_init(void) 66 | { 67 | // set default options before checking options 68 | const char* dest_ip = LOCALHOST_IP; 69 | uint8_t my_sys_id = DEFAULT_SYS_ID; 70 | uint16_t port = RC_MAV_DEFAULT_UDP_PORT; 71 | 72 | // initialize the UDP port and listening thread with the rc_mav lib 73 | if (rc_mav_init(settings.my_sys_id, settings.dest_ip, settings.mav_port, 74 | RC_MAV_DEFAULT_CONNECTION_TIMEOUT_US) < 0) 75 | return -1; 76 | 77 | // set the mocap callback to record position 78 | rc_mav_set_callback(MAVLINK_MSG_ID_ATT_POS_MOCAP, __callback_func_mocap); 79 | return 0; 80 | } 81 | 82 | int cleanup_mavlink_manager(void) 83 | { 84 | return rc_mav_cleanup(); 85 | } -------------------------------------------------------------------------------- /src/mix.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file mixing_matrix.c 3 | */ 4 | 5 | #include // for DBL_MAX 6 | #include 7 | #include 8 | #include 9 | 10 | // clang-format off 11 | /** 12 | * Most popular: 4-rotor X layout like DJI Phantom and 3DR Iris 13 | * top view: 14 | * 4 1 cw ccw X Z down 15 | * X ^ 16 | * 3 2 ccw cw + > Y 17 | * 18 | * columns: X Y Z Roll Pitch Yaw 19 | * rows: motors 1-4 20 | */ 21 | static double mix_4x[][6] = { 22 | {0.0, 0.0, -1.0, -0.5, 0.5, 0.5}, 23 | {0.0, 0.0, -1.0, -0.5, -0.5, -0.5}, 24 | {0.0, 0.0, -1.0, 0.5, -0.5, 0.5}, 25 | {0.0, 0.0, -1.0, 0.5, 0.5, -0.5}}; 26 | 27 | /** 28 | * less popular: 4-rotor + layout 29 | * 30 | * top view: 31 | * 1 ccw X Z down 32 | * 4 + 2 cw cw ^ 33 | * 3 ccw + > Y 34 | * columns: X Y Z Roll Pitch Yaw 35 | * rows: motors 1-4 36 | */ 37 | static double mix_4plus[][6] = { 38 | {0.0, 0.0, -1.0, 0.0, 0.5, 0.5}, 39 | {0.0, 0.0, -1.0, -0.5, 0.0, -0.5}, 40 | {0.0, 0.0, -1.0, 0.0, -0.5, 0.5}, 41 | {0.0, 0.0, -1.0, 0.5, 0.0, -0.5}}; 42 | 43 | /* 44 | * 6X like DJI S800 45 | * 46 | * top view: 47 | * 6 1 cw ccw X Z down 48 | * 5 2 ccw cw ^ 49 | * 4 3 cw ccw + > Y 50 | * columns: X Y Z Roll Pitch Yaw 51 | * rows: motors 1-6 52 | */ 53 | static double mix_6x[][6] = { 54 | {0.0, 0.0, -1.0, -0.25, 0.5, 0.5}, 55 | {0.0, 0.0, -1.0, -0.50, 0.0, -0.5}, 56 | {0.0, 0.0, -1.0, -0.25, -0.5, 0.5}, 57 | {0.0, 0.0, -1.0, 0.25, -0.5, -0.5}, 58 | {0.0, 0.0, -1.0, 0.50, 0.0, 0.5}, 59 | {0.0, 0.0, -1.0, 0.25, 0.5, -0.5}}; 60 | 61 | /** 62 | * 8X like DJI S1000 63 | * 64 | * 8 1 cw ccw 65 | * 7 2 ccw cw X Z down 66 | * 6 3 cw ccw ^ 67 | * 5 4 ccw cw + > Y 68 | * 69 | * columns: X Y Z Roll Pitch Yaw 70 | * rows: motors 1-8 71 | */ 72 | static double mix_8x[][6] = { 73 | {0.0, 0.0, -1.0, -0.21, 0.50, 0.5}, 74 | {0.0, 0.0, -1.0, -0.50, 0.21, -0.5}, 75 | {0.0, 0.0, -1.0, -0.50, -0.21, 0.5}, 76 | {0.0, 0.0, -1.0, -0.21, -0.50, -0.5}, 77 | {0.0, 0.0, -1.0, 0.21, -0.50, 0.5}, 78 | {0.0, 0.0, -1.0, 0.50, -0.21, -0.5}, 79 | {0.0, 0.0, -1.0, 0.50, 0.21, 0.5}, 80 | {0.0, 0.0, -1.0, 0.21, 0.50, -0.5}}; 81 | 82 | /** 83 | * 6D0F control for rotorbits platform 84 | * 85 | * 6 1 cw ccw X 86 | * 5 2 ccw cw ^ 87 | * 4 3 cw ccw + > Y 88 | * 89 | * columns: X Y Z Roll Pitch Yaw 90 | * rows: motors 1-6 91 | */ 92 | static double mix_6dof_rotorbits[][6] = { 93 | {-0.2736, 0.3638, -1.0000, -0.2293, 0.3921, 0.3443}, 94 | {0.6362, 0.0186, -1.0000, -0.3638, -0.0297, -0.3638}, 95 | {-0.3382, -0.3533, -1.0000, -0.3320, -0.3638, 0.3546}, 96 | {-0.3382, 0.3533, -1.0000, 0.3320, -0.3638, -0.3546}, 97 | {0.6362, -0.0186, -1.0000, 0.3638, -0.0297, 0.3638}, 98 | {-0.2736, -0.3638, -1.0000, 0.2293, 0.3921, -0.3443}}; 99 | 100 | /** 101 | * 6D0F control for 5-inch nylon monocoque 102 | * 103 | * 6 1 cw ccw X 104 | * 5 2 ccw cw ^ 105 | * 4 3 cw ccw + > Y 106 | * 107 | * columns: X Y Z Roll Pitch Yaw 108 | * rows: motors 1-6 109 | */ 110 | static double mix_6dof_5inch_monocoque[][6] = { 111 | {-0.2296, 0.2296, -1.0000, -0.2289, 0.2296, 0.2221}, 112 | {0.4742, 0.0000, -1.0000, -0.2296, -0.0000, -0.2296}, 113 | {-0.2296, -0.2296, -1.0000, -0.2289, -0.2296, 0.2221}, 114 | {-0.2296, 0.2296, -1.0000, 0.2289, -0.2296, -0.2221}, 115 | {0.4742, -0.0000, -1.0000, 0.2296, -0.0000, 0.2296}, 116 | {-0.2296, -0.2296, -1.0000, 0.2289, 0.2296, -0.2221}}; 117 | 118 | // clang-format off 119 | 120 | static double (*mix_matrix)[6]; 121 | static int initialized; 122 | static int rotors; 123 | static int dof; 124 | 125 | int mix_init(rotor_layout_t layout) 126 | { 127 | switch (layout) 128 | { 129 | case LAYOUT_4X: 130 | rotors = 4; 131 | dof = 4; 132 | mix_matrix = mix_4x; 133 | break; 134 | case LAYOUT_4PLUS: 135 | rotors = 4; 136 | dof = 4; 137 | mix_matrix = mix_4plus; 138 | break; 139 | case LAYOUT_6X: 140 | rotors = 6; 141 | dof = 4; 142 | mix_matrix = mix_6x; 143 | break; 144 | case LAYOUT_8X: 145 | rotors = 8; 146 | dof = 4; 147 | mix_matrix = mix_8x; 148 | break; 149 | case LAYOUT_6DOF_ROTORBITS: 150 | rotors = 6; 151 | dof = 6; 152 | mix_matrix = mix_6dof_rotorbits; 153 | break; 154 | case LAYOUT_6DOF_5INCH_MONOCOQUE: 155 | rotors = 6; 156 | dof = 6; 157 | mix_matrix = mix_6dof_5inch_monocoque; 158 | break; 159 | default: 160 | fprintf(stderr, "ERROR in mix_init() unknown rotor layout\n"); 161 | return -1; 162 | } 163 | 164 | initialized = 1; 165 | return 0; 166 | } 167 | 168 | int mix_all_controls(double u[6], double* mot) 169 | { 170 | int i, j; 171 | if (initialized != 1) 172 | { 173 | fprintf(stderr, "ERROR in mix_all_controls, mixing matrix not set yet\n"); 174 | return -1; 175 | } 176 | // sum control inputs 177 | for (i = 0; i < rotors; i++) 178 | { 179 | mot[i] = 0.0; 180 | for (j = 0; j < 6; j++) 181 | { 182 | mot[i] += mix_matrix[i][j] * u[j]; 183 | } 184 | } 185 | // ensure saturation, should not need to do this if mix_check_saturation 186 | // was used properly, but here for safety anyway. 187 | for (i = 0; i < rotors; i++) 188 | { 189 | if (mot[i] > 1.0) 190 | mot[i] = 1.0; 191 | else if (mot[i] < 0.0) 192 | mot[i] = 0.0; 193 | } 194 | return 0; 195 | } 196 | 197 | int mix_check_saturation(int ch, double* mot, double* min, double* max) 198 | { 199 | int i, min_ch; 200 | double tmp; 201 | double new_max = DBL_MAX; 202 | double new_min = -DBL_MAX; 203 | 204 | if (initialized != 1) 205 | { 206 | fprintf(stderr, "ERROR: in check_channel_saturation, mix matrix not set yet\n"); 207 | return -1; 208 | } 209 | 210 | switch (dof) 211 | { 212 | case 4: 213 | min_ch = 2; 214 | break; 215 | case 6: 216 | min_ch = 0; 217 | break; 218 | default: 219 | fprintf(stderr, 220 | "ERROR: in check_channel_saturation, dof should be 4 or 6, currently %d\n", dof); 221 | return -1; 222 | } 223 | 224 | if (ch < min_ch || ch >= 6) 225 | { 226 | fprintf(stderr, "ERROR: in check_channel_saturation, ch out of bounds\n"); 227 | return -1; 228 | } 229 | 230 | // make sure motors are not already saturated 231 | for (i = 0; i < rotors; i++) 232 | { 233 | if (mot[i] > 1.0 || mot[i] < 0.0) 234 | { 235 | fprintf(stderr, "ERROR: motor channel already out of bounds\n"); 236 | return -1; 237 | } 238 | } 239 | 240 | // find max positive input 241 | for (i = 0; i < rotors; i++) 242 | { 243 | // if mix channel is 0, impossible to saturate 244 | if (mix_matrix[i][ch] == 0.0) continue; 245 | // for positive entry in mix matrix 246 | if (mix_matrix[i][ch] > 0.0) tmp = (1.0 - mot[i]) / mix_matrix[i][ch]; 247 | // for negative entry in mix matrix 248 | else 249 | tmp = -mot[i] / mix_matrix[i][ch]; 250 | // set new upper limit if lower than current 251 | if (tmp < new_max) new_max = tmp; 252 | } 253 | 254 | // find min (most negative) input 255 | for (i = 0; i < rotors; i++) 256 | { 257 | // if mix channel is 0, impossible to saturate 258 | if (mix_matrix[i][ch] == 0.0) continue; 259 | // for positive entry in mix matrix 260 | if (mix_matrix[i][ch] > 0.0) tmp = -mot[i] / mix_matrix[i][ch]; 261 | // for negative entry in mix matrix 262 | else 263 | tmp = (1.0 - mot[i]) / mix_matrix[i][ch]; 264 | // set new upper limit if lower than current 265 | if (tmp > new_min) new_min = tmp; 266 | } 267 | 268 | *min = new_min; 269 | *max = new_max; 270 | return 0; 271 | } 272 | 273 | int mix_add_input(double u, int ch, double* mot) 274 | { 275 | int i; 276 | int min_ch; 277 | 278 | if (initialized != 1 || dof == 0) 279 | { 280 | fprintf(stderr, "ERROR: in mix_add_input, mix matrix not set yet\n"); 281 | return -1; 282 | } 283 | switch (dof) 284 | { 285 | case 4: 286 | min_ch = 2; 287 | break; 288 | case 6: 289 | min_ch = 0; 290 | break; 291 | default: 292 | fprintf(stderr, "ERROR: in mix_add_input, dof should be 4 or 6, currently %d\n", dof); 293 | return -1; 294 | } 295 | 296 | if (ch < min_ch || ch >= 6) 297 | { 298 | fprintf(stderr, "ERROR: in mix_add_input, ch out of bounds\n"); 299 | return -1; 300 | } 301 | 302 | // add inputs 303 | for (i = 0; i < rotors; i++) 304 | { 305 | mot[i] += u * mix_matrix[i][ch]; 306 | // ensure saturation, should not need to do this if mix_check_saturation 307 | // was used properly, but here for safety anyway. 308 | if (mot[i] > 1.0) 309 | mot[i] = 1.0; 310 | else if (mot[i] < 0.0) 311 | mot[i] = 0.0; 312 | } 313 | return 0; 314 | } 315 | -------------------------------------------------------------------------------- /src/printf_manager.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file printf_manager.c 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | static pthread_t printf_manager_thread; 23 | static int initialized = 0; 24 | 25 | const char* const colours[] = {KYEL, KCYN, KGRN, KMAG}; 26 | const int num_colours = 4; // length of above array 27 | int current_colour = 0; 28 | 29 | /** 30 | * @brief { function_description } 31 | * 32 | * @return string with ascii colour code 33 | */ 34 | static const char* __next_colour() 35 | { 36 | // if reached the end of the colour list, loop around 37 | if (current_colour >= (num_colours - 1)) 38 | { 39 | current_colour = 0; 40 | return colours[num_colours - 1]; 41 | } 42 | // else increment counter and return 43 | current_colour++; 44 | return colours[current_colour - 1]; 45 | } 46 | 47 | static void __reset_colour() 48 | { 49 | current_colour = 0; 50 | } 51 | 52 | static int __print_header() 53 | { 54 | int i; 55 | 56 | printf("\n"); 57 | __reset_colour(); 58 | if (settings.printf_arm) 59 | { 60 | printf(" arm |"); 61 | } 62 | if (settings.printf_altitude) 63 | { 64 | printf("%s alt(m) |altdot|", __next_colour()); 65 | } 66 | if (settings.printf_rpy) 67 | { 68 | printf("%s roll|pitch| yaw |", __next_colour()); 69 | } 70 | if (settings.printf_sticks) 71 | { 72 | printf("%s kill | thr |roll |pitch| yaw |", __next_colour()); 73 | } 74 | if (settings.printf_setpoint) 75 | { 76 | printf("%s sp_a | sp_r| sp_p| sp_y|", __next_colour()); 77 | } 78 | if (settings.printf_u) 79 | { 80 | printf("%s U0X | U1Y | U2Z | U3r | U4p | U5y |", __next_colour()); 81 | } 82 | if (settings.printf_motors) 83 | { 84 | printf("%s", __next_colour()); 85 | for (i = 0; i < settings.num_rotors; i++) 86 | { 87 | printf(" M%d |", i + 1); 88 | } 89 | } 90 | printf(KNRM); 91 | if (settings.printf_mode) 92 | { 93 | printf(" MODE "); 94 | } 95 | 96 | printf("\n"); 97 | fflush(stdout); 98 | return 0; 99 | } 100 | 101 | static void* __printf_manager_func(__attribute__((unused)) void* ptr) 102 | { 103 | arm_state_t prev_arm_state; 104 | int i; 105 | initialized = 1; 106 | printf("\nTurn your transmitter kill switch to arm.\n"); 107 | printf("Then move throttle UP then DOWN to arm controller\n\n"); 108 | 109 | // turn off linewrap to avoid runaway prints 110 | printf(WRAP_DISABLE); 111 | 112 | // print the header 113 | __print_header(); 114 | 115 | prev_arm_state = fstate.arm_state; 116 | 117 | // sleep so state_estimator can run first 118 | rc_usleep(100000); 119 | 120 | while (rc_get_state() != EXITING) 121 | { 122 | // re-print header on disarming 123 | // if(fstate.arm_state==DISARMED && prev_arm_state==ARMED){ 124 | // __print_header(); 125 | //} 126 | 127 | printf("\r"); 128 | if (settings.printf_arm) 129 | { 130 | if (fstate.arm_state == ARMED) 131 | printf("%s ARMED %s |", KRED, KNRM); 132 | else 133 | printf("%sDISARMED%s|", KGRN, KNRM); 134 | } 135 | __reset_colour(); 136 | if (settings.printf_altitude) 137 | { 138 | printf("%s%+5.2f |%+5.2f |", __next_colour(), state_estimate.alt_bmp, 139 | state_estimate.alt_bmp_vel); 140 | } 141 | if (settings.printf_rpy) 142 | { 143 | printf(KCYN); 144 | printf("%s%+5.2f|%+5.2f|%+5.2f|", __next_colour(), state_estimate.roll, 145 | state_estimate.pitch, state_estimate.continuous_yaw); 146 | } 147 | if (settings.printf_sticks) 148 | { 149 | if (user_input.requested_arm_mode == ARMED) 150 | printf("%s ARMED ", KRED); 151 | else 152 | printf("%sDISARMED", KGRN); 153 | printf(KGRN); 154 | printf("%s|%+5.2f|%+5.2f|%+5.2f|%+5.2f|", __next_colour(), user_input.thr_stick, 155 | user_input.roll_stick, user_input.pitch_stick, user_input.yaw_stick); 156 | } 157 | if (settings.printf_setpoint) 158 | { 159 | printf("%s%+5.2f|%+5.2f|%+5.2f|%+5.2f|", __next_colour(), setpoint.Z, setpoint.roll, 160 | setpoint.pitch, setpoint.yaw); 161 | } 162 | if (settings.printf_u) 163 | { 164 | printf("%s%+5.2f|%+5.2f|%+5.2f|%+5.2f|%+5.2f|%+5.2f|", __next_colour(), fstate.u[0], 165 | fstate.u[1], fstate.u[2], fstate.u[3], fstate.u[4], fstate.u[5]); 166 | } 167 | if (settings.printf_motors) 168 | { 169 | printf("%s", __next_colour()); 170 | for (i = 0; i < settings.num_rotors; i++) 171 | { 172 | printf("%+5.2f|", fstate.m[i]); 173 | } 174 | } 175 | printf(KNRM); 176 | if (settings.printf_mode) 177 | { 178 | print_flight_mode(user_input.flight_mode); 179 | } 180 | 181 | fflush(stdout); 182 | prev_arm_state = fstate.arm_state; 183 | rc_usleep(1000000 / PRINTF_MANAGER_HZ); 184 | } 185 | 186 | // put linewrap back on 187 | printf(WRAP_ENABLE); 188 | 189 | return NULL; 190 | } 191 | 192 | int printf_init() 193 | { 194 | if (rc_pthread_create(&printf_manager_thread, __printf_manager_func, NULL, SCHED_FIFO, 195 | PRINTF_MANAGER_PRI) == -1) 196 | { 197 | fprintf(stderr, "ERROR in start_printf_manager, failed to start thread\n"); 198 | return -1; 199 | } 200 | rc_usleep(50000); 201 | return 0; 202 | } 203 | 204 | int printf_cleanup() 205 | { 206 | int ret = 0; 207 | if (initialized) 208 | { 209 | // wait for the thread to exit 210 | ret = rc_pthread_timed_join(printf_manager_thread, NULL, PRINTF_MANAGER_TOUT); 211 | if (ret == 1) 212 | fprintf(stderr, "WARNING: printf_manager_thread exit timeout\n"); 213 | else if (ret == -1) 214 | fprintf(stderr, "ERROR: failed to join printf_manager thread\n"); 215 | } 216 | initialized = 0; 217 | return ret; 218 | } 219 | 220 | int print_flight_mode(flight_mode_t mode) 221 | { 222 | switch (mode) 223 | { 224 | case TEST_BENCH_4DOF: 225 | printf("%sTEST_BENCH_4DOF%s", KYEL, KNRM); 226 | return 0; 227 | case TEST_BENCH_6DOF: 228 | printf("%sTEST_BENCH_6DOF%s", KYEL, KNRM); 229 | return 0; 230 | case DIRECT_THROTTLE_4DOF: 231 | printf("%sDIR_THRTLE_4DOF%s", KCYN, KNRM); 232 | return 0; 233 | case DIRECT_THROTTLE_6DOF: 234 | printf("%sDIR_THRTLE_6DOF%s", KCYN, KNRM); 235 | return 0; 236 | case ALT_HOLD_4DOF: 237 | printf("%sALT_HOLD_4DOF %s", KBLU, KNRM); 238 | return 0; 239 | case ALT_HOLD_6DOF: 240 | printf("%sALT_HOLD_6DOF %s", KBLU, KNRM); 241 | return 0; 242 | default: 243 | fprintf(stderr, "ERROR in print_flight_mode, unknown flight mode\n"); 244 | return -1; 245 | } 246 | } 247 | -------------------------------------------------------------------------------- /src/setpoint_manager.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file setpoint_manager.c 3 | * 4 | * 5 | **/ 6 | #include 7 | #include 8 | #include // for memset 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define XYZ_MAX_ERROR 0.5 ///< meters. 21 | 22 | setpoint_t setpoint; // extern variable in setpoint_manager.h 23 | 24 | void __update_yaw(void) 25 | { 26 | // if throttle stick is down all the way, probably landed, so 27 | // keep the yaw setpoint at current yaw so it takes off straight 28 | if (user_input.thr_stick < -0.95) 29 | { 30 | setpoint.yaw = state_estimate.yaw; 31 | setpoint.yaw_dot = 0.0; 32 | return; 33 | } 34 | // otherwise, scale yaw_rate by max yaw rate in rad/s 35 | // and move yaw setpoint 36 | setpoint.yaw_dot = user_input.yaw_stick * MAX_YAW_RATE; 37 | setpoint.yaw += setpoint.yaw_dot * DT; 38 | return; 39 | } 40 | 41 | void __update_Z(void) 42 | { 43 | // make sure setpoint doesn't go too far below current altitude since we 44 | // can't sink into the ground 45 | if (setpoint.Z > (state_estimate.Z + XYZ_MAX_ERROR)) 46 | { 47 | setpoint.Z = state_estimate.Z + XYZ_MAX_ERROR; 48 | setpoint.Z_dot = 0.0; 49 | return; 50 | } 51 | setpoint.Z_dot = -user_input.thr_stick * settings.max_Z_velocity; 52 | setpoint.Z += setpoint.Z_dot * DT; 53 | return; 54 | } 55 | 56 | void __update_XY_pos(void) 57 | { 58 | // make sure setpoint doesn't go too far from state in case touching something 59 | if (setpoint.X > (state_estimate.X + XYZ_MAX_ERROR)) 60 | { 61 | setpoint.X = state_estimate.X + XYZ_MAX_ERROR; 62 | setpoint.X_dot = 0.0; 63 | } 64 | else if (setpoint.X < (state_estimate.X - XYZ_MAX_ERROR)) 65 | { 66 | setpoint.X = state_estimate.X - XYZ_MAX_ERROR; 67 | setpoint.X_dot = 0.0; 68 | return; 69 | } 70 | else 71 | { 72 | setpoint.X += setpoint.X_dot * DT; 73 | } 74 | 75 | if (setpoint.Y > (state_estimate.Y + XYZ_MAX_ERROR)) 76 | { 77 | setpoint.Y = state_estimate.Y + XYZ_MAX_ERROR; 78 | setpoint.Y_dot = 0.0; 79 | return; 80 | } 81 | else if (setpoint.Y < (state_estimate.Y - XYZ_MAX_ERROR)) 82 | { 83 | setpoint.Y = state_estimate.Y - XYZ_MAX_ERROR; 84 | setpoint.Y_dot = 0.0; 85 | return; 86 | } 87 | else 88 | { 89 | setpoint.Y += setpoint.Y_dot * DT; 90 | } 91 | 92 | return; 93 | } 94 | 95 | int setpoint_manager_init(void) 96 | { 97 | if (setpoint.initialized) 98 | { 99 | fprintf(stderr, "ERROR in setpoint_manager_init, already initialized\n"); 100 | return -1; 101 | } 102 | memset(&setpoint, 0, sizeof(setpoint_t)); 103 | setpoint.initialized = 1; 104 | return 0; 105 | } 106 | 107 | int setpoint_manager_update(void) 108 | { 109 | if (setpoint.initialized == 0) 110 | { 111 | fprintf(stderr, "ERROR in setpoint_manager_update, not initialized yet\n"); 112 | return -1; 113 | } 114 | 115 | if (user_input.initialized == 0) 116 | { 117 | fprintf(stderr, "ERROR in setpoint_manager_update, input_manager not initialized yet\n"); 118 | return -1; 119 | } 120 | 121 | // if PAUSED or UNINITIALIZED, do nothing 122 | if (rc_get_state() != RUNNING) return 0; 123 | 124 | // shutdown feedback on kill switch 125 | if (user_input.requested_arm_mode == DISARMED) 126 | { 127 | if (fstate.arm_state == ARMED) feedback_disarm(); 128 | return 0; 129 | } 130 | 131 | // finally, switch between flight modes and adjust setpoint properly 132 | switch (user_input.flight_mode) 133 | { 134 | case TEST_BENCH_4DOF: 135 | // configure which controllers are enabled 136 | setpoint.en_6dof = 0; 137 | setpoint.en_rpy_ctrl = 0; 138 | setpoint.en_Z_ctrl = 0; 139 | setpoint.en_XY_vel_ctrl = 0; 140 | setpoint.en_XY_pos_ctrl = 0; 141 | 142 | setpoint.roll_throttle = user_input.roll_stick; 143 | setpoint.pitch_throttle = user_input.pitch_stick; 144 | setpoint.yaw_throttle = user_input.yaw_stick; 145 | setpoint.Z_throttle = -user_input.thr_stick; 146 | // TODO add these two throttle modes as options to settings, I use a radio 147 | // with self-centering throttle so having 0 in the middle is safest 148 | // setpoint.Z_throttle = -(user_input.thr_stick+1.0)/2.0; 149 | break; 150 | 151 | case TEST_BENCH_6DOF: 152 | setpoint.en_6dof = 1; 153 | setpoint.en_rpy_ctrl = 0; 154 | setpoint.en_Z_ctrl = 0; 155 | setpoint.en_XY_vel_ctrl = 0; 156 | setpoint.en_XY_pos_ctrl = 0; 157 | 158 | setpoint.X_throttle = -user_input.pitch_stick; 159 | setpoint.Y_throttle = user_input.roll_stick; 160 | setpoint.roll_throttle = 0.0; 161 | setpoint.pitch_throttle = 0.0; 162 | setpoint.yaw_throttle = user_input.yaw_stick; 163 | setpoint.Z_throttle = -user_input.thr_stick; 164 | break; 165 | 166 | case DIRECT_THROTTLE_4DOF: 167 | setpoint.en_6dof = 0; 168 | setpoint.en_rpy_ctrl = 1; 169 | setpoint.en_Z_ctrl = 0; 170 | setpoint.en_XY_vel_ctrl = 0; 171 | setpoint.en_XY_pos_ctrl = 0; 172 | 173 | setpoint.roll = user_input.roll_stick; 174 | setpoint.pitch = user_input.pitch_stick; 175 | setpoint.Z_throttle = -user_input.thr_stick; 176 | __update_yaw(); 177 | break; 178 | 179 | case DIRECT_THROTTLE_6DOF: 180 | setpoint.en_6dof = 1; 181 | setpoint.en_rpy_ctrl = 1; 182 | setpoint.en_Z_ctrl = 0; 183 | setpoint.en_XY_vel_ctrl = 0; 184 | setpoint.en_XY_pos_ctrl = 0; 185 | 186 | setpoint.X_throttle = -user_input.pitch_stick; 187 | setpoint.Y_throttle = user_input.roll_stick; 188 | setpoint.Z_throttle = -user_input.thr_stick; 189 | __update_yaw(); 190 | break; 191 | 192 | case ALT_HOLD_4DOF: 193 | setpoint.en_6dof = 0; 194 | setpoint.en_rpy_ctrl = 1; 195 | setpoint.en_Z_ctrl = 1; 196 | setpoint.en_XY_vel_ctrl = 0; 197 | setpoint.en_XY_pos_ctrl = 0; 198 | 199 | setpoint.roll = user_input.roll_stick; 200 | setpoint.pitch = user_input.pitch_stick; 201 | __update_Z(); 202 | __update_yaw(); 203 | break; 204 | 205 | case ALT_HOLD_6DOF: 206 | setpoint.en_6dof = 1; 207 | setpoint.en_rpy_ctrl = 1; 208 | setpoint.en_Z_ctrl = 1; 209 | setpoint.en_XY_vel_ctrl = 0; 210 | setpoint.en_XY_pos_ctrl = 0; 211 | 212 | setpoint.roll = 0.0; 213 | setpoint.pitch = 0.0; 214 | setpoint.X_throttle = -user_input.pitch_stick; 215 | setpoint.Y_throttle = user_input.roll_stick; 216 | __update_Z(); 217 | __update_yaw(); 218 | break; 219 | 220 | case VELOCITY_CONTROL_4DOF: 221 | setpoint.en_6dof = 0; 222 | setpoint.en_rpy_ctrl = 1; 223 | setpoint.en_Z_ctrl = 1; 224 | setpoint.en_XY_vel_ctrl = 1; 225 | setpoint.en_XY_pos_ctrl = 0; 226 | 227 | setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; 228 | setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; 229 | __update_Z(); 230 | __update_yaw(); 231 | break; 232 | 233 | case VELOCITY_CONTROL_6DOF: 234 | setpoint.en_6dof = 1; 235 | setpoint.en_rpy_ctrl = 1; 236 | setpoint.en_Z_ctrl = 1; 237 | setpoint.en_XY_vel_ctrl = 1; 238 | setpoint.en_XY_pos_ctrl = 0; 239 | 240 | setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; 241 | setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; 242 | __update_Z(); 243 | __update_yaw(); 244 | break; 245 | 246 | case POSITION_CONTROL_4DOF: 247 | setpoint.en_6dof = 0; 248 | setpoint.en_rpy_ctrl = 1; 249 | setpoint.en_Z_ctrl = 1; 250 | setpoint.en_XY_vel_ctrl = 0; 251 | setpoint.en_XY_pos_ctrl = 1; 252 | 253 | setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; 254 | setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; 255 | __update_XY_pos(); 256 | __update_Z(); 257 | __update_yaw(); 258 | break; 259 | 260 | case POSITION_CONTROL_6DOF: 261 | setpoint.en_6dof = 1; 262 | setpoint.en_rpy_ctrl = 1; 263 | setpoint.en_Z_ctrl = 1; 264 | setpoint.en_XY_vel_ctrl = 0; 265 | setpoint.en_XY_pos_ctrl = 1; 266 | 267 | setpoint.X_dot = -user_input.pitch_stick * settings.max_XY_velocity; 268 | setpoint.Y_dot = user_input.roll_stick * settings.max_XY_velocity; 269 | __update_XY_pos(); 270 | __update_Z(); 271 | __update_yaw(); 272 | break; 273 | 274 | default: // should never get here 275 | fprintf(stderr, "ERROR in setpoint_manager thread, unknown flight mode\n"); 276 | break; 277 | 278 | } // end switch(user_input.flight_mode) 279 | 280 | // arm feedback when requested 281 | if (user_input.requested_arm_mode == ARMED) 282 | { 283 | if (fstate.arm_state == DISARMED) feedback_arm(); 284 | } 285 | 286 | return 0; 287 | } 288 | 289 | int setpoint_manager_cleanup(void) 290 | { 291 | setpoint.initialized = 0; 292 | return 0; 293 | } 294 | -------------------------------------------------------------------------------- /src/settings.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file settings.c 3 | * 4 | * contains all the functions for io to the settings file, including default 5 | * values that can be written to disk if no file is present. 6 | **/ 7 | 8 | #include // for F_OK 9 | #include 10 | #include // FOR str_cmp() 11 | #include // for access() 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | // json object respresentation of the whole settings file 20 | static json_object* jobj; 21 | 22 | // primary settings struct declared as extern in header is defined ONCE here 23 | settings_t settings; 24 | 25 | // if anything goes wrong set this flag back to 0 26 | static int was_load_successful = 0; 27 | 28 | //////////////////////////////////////////////////////////////////////////////// 29 | /// MACROS FOR PARSING JSON TYPES 30 | //////////////////////////////////////////////////////////////////////////////// 31 | 32 | // macro for reading a boolean 33 | #define PARSE_BOOL(name) \ 34 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 35 | { \ 36 | fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ 37 | return -1; \ 38 | } \ 39 | if (json_object_is_type(tmp, json_type_boolean) == 0) \ 40 | { \ 41 | fprintf(stderr, "ERROR parsing settings file, " #name " should be a boolean\n"); \ 42 | return -1; \ 43 | } \ 44 | settings.name = json_object_get_boolean(tmp); 45 | 46 | // macro for reading an integer 47 | #define PARSE_INT(name) \ 48 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 49 | { \ 50 | fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ 51 | return -1; \ 52 | } \ 53 | if (json_object_is_type(tmp, json_type_int) == 0) \ 54 | { \ 55 | fprintf(stderr, "ERROR parsing settings file, " #name " should be an int\n"); \ 56 | return -1; \ 57 | } \ 58 | settings.name = json_object_get_int(tmp); 59 | 60 | // macro for reading a bound integer 61 | #define PARSE_INT_MIN_MAX(name, min, max) \ 62 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 63 | { \ 64 | fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ 65 | return -1; \ 66 | } \ 67 | if (json_object_is_type(tmp, json_type_int) == 0) \ 68 | { \ 69 | fprintf(stderr, "ERROR parsing settings file, " #name " should be an int\n"); \ 70 | return -1; \ 71 | } \ 72 | settings.name = json_object_get_int(tmp); \ 73 | if (settings.name < min || settings.name > max) \ 74 | { \ 75 | fprintf(stderr, "ERROR parsing settings file, " #name " should be between min and max\n"); \ 76 | return -1; \ 77 | } 78 | 79 | // macro for reading a polarity which should be +-1 80 | #define PARSE_POLARITY(name) \ 81 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 82 | { \ 83 | fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ 84 | return -1; \ 85 | } \ 86 | if (json_object_is_type(tmp, json_type_int) == 0) \ 87 | { \ 88 | fprintf(stderr, "ERROR parsing settings file, " #name " should be an int\n"); \ 89 | return -1; \ 90 | } \ 91 | settings.name = json_object_get_int(tmp); \ 92 | if (settings.name != -1 && settings.name != 1) \ 93 | { \ 94 | fprintf(stderr, "ERROR parsing settings file, " #name " should be -1 or 1\n"); \ 95 | return -1; \ 96 | } 97 | 98 | // macro for reading a floating point number 99 | #define PARSE_DOUBLE_MIN_MAX(name, min, max) \ 100 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 101 | { \ 102 | fprintf(stderr, "ERROR can't find " #name " in settings file\n"); \ 103 | return -1; \ 104 | } \ 105 | if (json_object_is_type(tmp, json_type_double) == 0) \ 106 | { \ 107 | fprintf(stderr, "ERROR " #name " should be a double\n"); \ 108 | return -1; \ 109 | } \ 110 | settings.name = json_object_get_double(tmp); \ 111 | if (settings.name < min || settings.name > max) \ 112 | { \ 113 | fprintf(stderr, "ERROR " #name " should be between min and max\n"); \ 114 | return -1; \ 115 | } 116 | 117 | // macro for reading a string 118 | #define PARSE_STRING(name) \ 119 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 120 | { \ 121 | fprintf(stderr, "ERROR parsing settings file, can't find " #name "\n"); \ 122 | return -1; \ 123 | } \ 124 | if (json_object_is_type(tmp, json_type_string) == 0) \ 125 | { \ 126 | fprintf(stderr, "ERROR parsing settings file, " #name " should be a string\n"); \ 127 | return -1; \ 128 | } \ 129 | strcpy(settings.name, json_object_get_string(tmp)); 130 | 131 | // macro for reading feedback controller 132 | #define PARSE_CONTROLLER(name) \ 133 | if (json_object_object_get_ex(jobj, #name, &tmp) == 0) \ 134 | { \ 135 | fprintf(stderr, "ERROR: can't find " #name " in settings file\n"); \ 136 | return -1; \ 137 | } \ 138 | if (__parse_controller(tmp, &settings.name)) \ 139 | { \ 140 | fprintf(stderr, "ERROR: could not parse " #name "\n"); \ 141 | return -1; \ 142 | } 143 | 144 | //////////////////////////////////////////////////////////////////////////////// 145 | /// functions for parsing enums 146 | //////////////////////////////////////////////////////////////////////////////// 147 | 148 | /** 149 | * @brief pulls rotor layout out of json object into settings struct 150 | * 151 | * @return 0 on success, -1 on failure 152 | */ 153 | static int __parse_layout(void) 154 | { 155 | struct json_object* tmp = NULL; 156 | char* tmp_str = NULL; 157 | if (json_object_object_get_ex(jobj, "layout", &tmp) == 0) 158 | { 159 | fprintf(stderr, "ERROR: can't find layout in settings file\n"); 160 | return -1; 161 | } 162 | if (json_object_is_type(tmp, json_type_string) == 0) 163 | { 164 | fprintf(stderr, "ERROR: layout should be a string\n"); 165 | return -1; 166 | } 167 | tmp_str = (char*)json_object_get_string(tmp); 168 | if (strcmp(tmp_str, "LAYOUT_6DOF_ROTORBITS") == 0) 169 | { 170 | settings.num_rotors = 6; 171 | settings.layout = LAYOUT_6DOF_ROTORBITS; 172 | } 173 | else if (strcmp(tmp_str, "LAYOUT_4X") == 0) 174 | { 175 | settings.num_rotors = 4; 176 | settings.layout = LAYOUT_4X; 177 | } 178 | else if (strcmp(tmp_str, "LAYOUT_4PLUS") == 0) 179 | { 180 | settings.num_rotors = 4; 181 | settings.layout = LAYOUT_4PLUS; 182 | } 183 | else if (strcmp(tmp_str, "LAYOUT_6X") == 0) 184 | { 185 | settings.num_rotors = 6; 186 | settings.layout = LAYOUT_6X; 187 | } 188 | else if (strcmp(tmp_str, "LAYOUT_8X") == 0) 189 | { 190 | settings.num_rotors = 8; 191 | settings.layout = LAYOUT_8X; 192 | } 193 | else 194 | { 195 | fprintf(stderr, "ERROR: invalid layout string\n"); 196 | return -1; 197 | } 198 | return 0; 199 | } 200 | 201 | static int __parse_thrust_map(void) 202 | { 203 | struct json_object* tmp = NULL; 204 | char* tmp_str = NULL; 205 | if (json_object_object_get_ex(jobj, "thrust_map", &tmp) == 0) 206 | { 207 | fprintf(stderr, "ERROR: can't find thrust_map in settings file\n"); 208 | return -1; 209 | } 210 | if (json_object_is_type(tmp, json_type_string) == 0) 211 | { 212 | fprintf(stderr, "ERROR: thrust map should be a string\n"); 213 | return -1; 214 | } 215 | tmp_str = (char*)json_object_get_string(tmp); 216 | if (strcmp(tmp_str, "LINEAR_MAP") == 0) 217 | { 218 | settings.thrust_map = LINEAR_MAP; 219 | } 220 | else if (strcmp(tmp_str, "MN1806_1400KV_4S") == 0) 221 | { 222 | settings.thrust_map = MN1806_1400KV_4S; 223 | } 224 | else if (strcmp(tmp_str, "F20_2300KV_2S") == 0) 225 | { 226 | settings.thrust_map = F20_2300KV_2S; 227 | } 228 | else if (strcmp(tmp_str, "RX2206_4S") == 0) 229 | { 230 | settings.thrust_map = RX2206_4S; 231 | } 232 | else 233 | { 234 | fprintf(stderr, "ERROR: invalid thrust_map string\n"); 235 | return -1; 236 | } 237 | return 0; 238 | } 239 | 240 | /** 241 | * @brief parses a json_object and fills in the flight mode. 242 | * 243 | * @param jobj The jobj to parse 244 | * @param mode pointer to write mode out to 245 | * 246 | * @return returns 0 on success or -1 on failure 247 | */ 248 | static int __parse_flight_mode(json_object* jobj_str, flight_mode_t* mode) 249 | { 250 | char* tmp_str = NULL; 251 | struct json_object* tmp = NULL; 252 | if (json_object_is_type(jobj_str, json_type_string) == 0) 253 | { 254 | fprintf(stderr, "ERROR: flight_mode should be a string\n"); 255 | return -1; 256 | } 257 | tmp_str = (char*)json_object_get_string(jobj_str); 258 | if (strcmp(tmp_str, "TEST_BENCH_4DOF") == 0) 259 | { 260 | *mode = TEST_BENCH_4DOF; 261 | } 262 | else if (strcmp(tmp_str, "TEST_BENCH_6DOF") == 0) 263 | { 264 | *mode = TEST_BENCH_6DOF; 265 | } 266 | else if (strcmp(tmp_str, "DIRECT_THROTTLE_4DOF") == 0) 267 | { 268 | *mode = DIRECT_THROTTLE_4DOF; 269 | } 270 | else if (strcmp(tmp_str, "DIRECT_THROTTLE_6DOF") == 0) 271 | { 272 | *mode = DIRECT_THROTTLE_6DOF; 273 | } 274 | else if (strcmp(tmp_str, "ALT_HOLD_4DOF") == 0) 275 | { 276 | *mode = ALT_HOLD_4DOF; 277 | } 278 | else if (strcmp(tmp_str, "ALT_HOLD_6DOF") == 0) 279 | { 280 | *mode = ALT_HOLD_6DOF; 281 | } 282 | else if (strcmp(tmp_str, "VELOCITY_CONTROL_4DOF") == 0) 283 | { 284 | *mode = VELOCITY_CONTROL_4DOF; 285 | } 286 | else if (strcmp(tmp_str, "VELOCITY_CONTROL_6DOF") == 0) 287 | { 288 | *mode = VELOCITY_CONTROL_6DOF; 289 | } 290 | else if (strcmp(tmp_str, "POSITION_CONTROL_4DOF") == 0) 291 | { 292 | *mode = POSITION_CONTROL_4DOF; 293 | } 294 | else if (strcmp(tmp_str, "POSITION_CONTROL_6DOF") == 0) 295 | { 296 | *mode = POSITION_CONTROL_6DOF; 297 | } 298 | else 299 | { 300 | fprintf(stderr, "ERROR: invalid flight mode\n"); 301 | return -1; 302 | } 303 | return 0; 304 | } 305 | 306 | static int __parse_kill_mode(void) 307 | { 308 | struct json_object* tmp = NULL; 309 | char* tmp_str = NULL; 310 | if (json_object_object_get_ex(jobj, "dsm_kill_mode", &tmp) == 0) 311 | { 312 | fprintf(stderr, "ERROR: can't find dsm_kill_mode in settings file\n"); 313 | return -1; 314 | } 315 | if (json_object_is_type(tmp, json_type_string) == 0) 316 | { 317 | fprintf(stderr, "ERROR: dsm_kill_mode should be a string\n"); 318 | return -1; 319 | } 320 | tmp_str = (char*)json_object_get_string(tmp); 321 | if (strcmp(tmp_str, "DSM_KILL_DEDICATED_SWITCH") == 0) 322 | { 323 | settings.dsm_kill_mode = DSM_KILL_DEDICATED_SWITCH; 324 | } 325 | else if (strcmp(tmp_str, "DSM_KILL_NEGATIVE_THROTTLE") == 0) 326 | { 327 | settings.dsm_kill_mode = DSM_KILL_NEGATIVE_THROTTLE; 328 | } 329 | else 330 | { 331 | fprintf(stderr, "ERROR: invalid dsm_kill_mode string\n"); 332 | return -1; 333 | } 334 | return 0; 335 | } 336 | 337 | /** 338 | * @ brief parses a json_object and sets up a new controller 339 | * 340 | * @param jobj The jobj to parse 341 | * @param filter pointer to write the new filter to 342 | * 343 | * @return 0 on success, -1 on failure 344 | */ 345 | static int __parse_controller(json_object* jobj_ctl, rc_filter_t* filter) 346 | { 347 | struct json_object* array = NULL; // to hold num & den arrays 348 | struct json_object* tmp = NULL; // temp object 349 | char* tmp_str = NULL; 350 | double tmp_flt, tmp_kp, tmp_ki, tmp_kd; 351 | int i, num_len, den_len; 352 | rc_vector_t num_vec = RC_VECTOR_INITIALIZER; 353 | rc_vector_t den_vec = RC_VECTOR_INITIALIZER; 354 | 355 | // destroy old memory in case the order changes 356 | rc_filter_free(filter); 357 | 358 | // pull out gain 359 | if (json_object_object_get_ex(jobj_ctl, "gain", &tmp) == 0) 360 | { 361 | fprintf(stderr, "ERROR: can't find controller gain in settings file\n"); 362 | return -1; 363 | } 364 | if (json_object_is_type(tmp, json_type_double) == 0) 365 | { 366 | fprintf(stderr, "ERROR: controller gain should be a double\n"); 367 | return -1; 368 | } 369 | tmp_flt = json_object_get_double(tmp); 370 | 371 | // check if PID gains or transfer function coefficients 372 | if (json_object_object_get_ex(jobj_ctl, "TF_or_PID", &tmp) == 0) 373 | { 374 | fprintf(stderr, "ERROR: can't find TF_or_PID in settings file\n"); 375 | return -1; 376 | } 377 | if (json_object_is_type(tmp, json_type_string) == 0) 378 | { 379 | fprintf(stderr, "ERROR: TF_or_PID should be a string\n"); 380 | return -1; 381 | } 382 | tmp_str = (char*)json_object_get_string(tmp); 383 | 384 | if (strcmp(tmp_str, "TF") == 0) 385 | { 386 | // pull out numerator 387 | if (json_object_object_get_ex(jobj_ctl, "numerator", &array) == 0) 388 | { 389 | fprintf(stderr, "ERROR: can't find controller numerator in settings file\n"); 390 | return -1; 391 | } 392 | if (json_object_is_type(array, json_type_array) == 0) 393 | { 394 | fprintf(stderr, "ERROR: controller numerator should be an array\n"); 395 | return -1; 396 | } 397 | num_len = json_object_array_length(array); 398 | if (num_len < 1) 399 | { 400 | fprintf(stderr, "ERROR, numerator must have at least 1 entry\n"); 401 | return -1; 402 | } 403 | rc_vector_alloc(&num_vec, num_len); 404 | for (i = 0; i < num_len; i++) 405 | { 406 | tmp = json_object_array_get_idx(array, i); 407 | if (json_object_is_type(tmp, json_type_double) == 0) 408 | { 409 | fprintf(stderr, "ERROR: numerator array entries should be a doubles\n"); 410 | return -1; 411 | } 412 | tmp_flt = json_object_get_double(tmp); 413 | num_vec.d[i] = tmp_flt; 414 | } 415 | 416 | // pull out denominator 417 | if (json_object_object_get_ex(jobj_ctl, "denominator", &array) == 0) 418 | { 419 | fprintf(stderr, "ERROR: can't find controller denominator in settings file\n"); 420 | return -1; 421 | } 422 | if (json_object_is_type(array, json_type_array) == 0) 423 | { 424 | fprintf(stderr, "ERROR: controller denominator should be an array\n"); 425 | return -1; 426 | } 427 | den_len = json_object_array_length(array); 428 | if (den_len < 1) 429 | { 430 | fprintf(stderr, "ERROR, denominator must have at least 1 entry\n"); 431 | return -1; 432 | } 433 | rc_vector_alloc(&den_vec, den_len); 434 | for (i = 0; i < den_len; i++) 435 | { 436 | tmp = json_object_array_get_idx(array, i); 437 | if (json_object_is_type(tmp, json_type_double) == 0) 438 | { 439 | fprintf(stderr, "ERROR: denominator array entries should be a doubles\n"); 440 | return -1; 441 | } 442 | tmp_flt = json_object_get_double(tmp); 443 | den_vec.d[i] = tmp_flt; 444 | } 445 | 446 | // check for improper TF 447 | if (num_len > den_len) 448 | { 449 | fprintf(stderr, "ERROR: improper transfer function\n"); 450 | rc_vector_free(&num_vec); 451 | rc_vector_free(&den_vec); 452 | return -1; 453 | } 454 | 455 | // check CT continuous time or DT discrete time 456 | if (json_object_object_get_ex(jobj_ctl, "CT_or_DT", &tmp) == 0) 457 | { 458 | fprintf(stderr, "ERROR: can't find CT_or_DT in settings file\n"); 459 | return -1; 460 | } 461 | if (json_object_is_type(tmp, json_type_string) == 0) 462 | { 463 | fprintf(stderr, "ERROR: CT_or_DT should be a string\n"); 464 | return -1; 465 | } 466 | tmp_str = (char*)json_object_get_string(tmp); 467 | 468 | // if CT, use tustin's approx to get to DT 469 | if (strcmp(tmp_str, "CT") == 0) 470 | { 471 | // get the crossover frequency 472 | if (json_object_object_get_ex(jobj_ctl, "crossover_freq_rad_per_sec", &tmp) == 0) 473 | { 474 | fprintf(stderr, "ERROR: can't find crossover frequency in settings file\n"); 475 | return -1; 476 | } 477 | if (json_object_is_type(tmp, json_type_double) == 0) 478 | { 479 | fprintf(stderr, "ERROR: crossover frequency should be a double\n"); 480 | return -1; 481 | } 482 | tmp_flt = json_object_get_double(tmp); 483 | if (rc_filter_c2d_tustin(filter, DT, num_vec, den_vec, tmp_flt)) 484 | { 485 | fprintf(stderr, "ERROR: failed to c2dtustin while parsing json\n"); 486 | return -1; 487 | } 488 | } 489 | 490 | // if DT, much easier, just construct filter 491 | else if (strcmp(tmp_str, "DT") == 0) 492 | { 493 | if (rc_filter_alloc(filter, num_vec, den_vec, DT)) 494 | { 495 | fprintf(stderr, "ERROR: failed to alloc filter in __parse_controller()"); 496 | return -1; 497 | } 498 | } 499 | 500 | // wrong value for CT_or_DT 501 | else 502 | { 503 | fprintf(stderr, "ERROR: CT_or_DT must be 'CT' or 'DT'\n"); 504 | printf("instead got :%s\n", tmp_str); 505 | return -1; 506 | } 507 | } 508 | 509 | else if (strcmp(tmp_str, "PID") == 0) 510 | { 511 | // pull out gains 512 | if (json_object_object_get_ex(jobj_ctl, "kp", &tmp) == 0) 513 | { 514 | fprintf(stderr, "ERROR: can't find kp in settings file\n"); 515 | return -1; 516 | } 517 | tmp_kp = json_object_get_double(tmp); 518 | if (json_object_object_get_ex(jobj_ctl, "ki", &tmp) == 0) 519 | { 520 | fprintf(stderr, "ERROR: can't find ki in settings file\n"); 521 | return -1; 522 | } 523 | tmp_ki = json_object_get_double(tmp); 524 | if (json_object_object_get_ex(jobj_ctl, "kd", &tmp) == 0) 525 | { 526 | fprintf(stderr, "ERROR: can't find kd in settings file\n"); 527 | return -1; 528 | } 529 | tmp_kd = json_object_get_double(tmp); 530 | // get the crossover frequency 531 | if (json_object_object_get_ex(jobj_ctl, "crossover_freq_rad_per_sec", &tmp) == 0) 532 | { 533 | fprintf(stderr, "ERROR: can't find crossover frequency in settings file\n"); 534 | return -1; 535 | } 536 | if (json_object_is_type(tmp, json_type_double) == 0) 537 | { 538 | fprintf(stderr, "ERROR: crossover frequency should be a double\n"); 539 | return -1; 540 | } 541 | tmp_flt = json_object_get_double(tmp); 542 | if (rc_filter_pid(filter, tmp_kp, tmp_ki, tmp_kd, 1.0 / tmp_flt, DT)) 543 | { 544 | fprintf(stderr, "ERROR: failed to alloc pid filter in __parse_controller()"); 545 | return -1; 546 | } 547 | } 548 | 549 | #ifdef DEBUG 550 | rc_filter_print(*filter); 551 | #endif 552 | 553 | rc_vector_free(&num_vec); 554 | rc_vector_free(&den_vec); 555 | 556 | return 0; 557 | } 558 | 559 | int settings_load_from_file(char* path) 560 | { 561 | struct json_object* tmp = NULL; // temp object 562 | char* tmp_str = NULL; // temp string poitner 563 | double tmp_flt; 564 | int tmp_int; 565 | 566 | was_load_successful = 0; 567 | 568 | #ifdef DEBUG 569 | fprintf(stderr, "beginning of load_settings_from_file\n"); 570 | fprintf(stderr, "about to check access of fly settings file\n"); 571 | #endif 572 | 573 | // read in file contents 574 | if (access(path, F_OK) != 0) 575 | { 576 | fprintf(stderr, "ERROR: settings file missing\n"); 577 | return -1; 578 | } 579 | else 580 | { 581 | #ifdef DEBUG 582 | printf("about to read json from file\n"); 583 | #endif 584 | jobj = json_object_from_file(path); 585 | if (jobj == NULL) 586 | { 587 | fprintf(stderr, "ERROR, failed to read settings from disk\n"); 588 | return -1; 589 | } 590 | } 591 | 592 | #ifdef DEBUG 593 | settings_print(); 594 | #endif 595 | 596 | // START PARSING 597 | 598 | PARSE_STRING(name) 599 | #ifdef DEBUG 600 | fprintf(stderr, "name: %s\n", settings.name); 601 | #endif 602 | PARSE_BOOL(warnings_en) 603 | #ifdef DEBUG 604 | fprintf(stderr, "warnings: %d\n", settings.warnings_en); 605 | #endif 606 | 607 | // PHYSICAL PARAMETERS 608 | // layout populates num_rotors, layout, and dof 609 | if (__parse_layout() == -1) return -1; // parse_layout also fill in num_rotors and dof 610 | #ifdef DEBUG 611 | fprintf(stderr, "layout:%d,%d\n", settings.layout, settings.num_rotors); 612 | #endif 613 | if (__parse_thrust_map() == -1) return -1; 614 | #ifdef DEBUG 615 | fprintf(stderr, "thrust_map: %d\n", settings.thrust_map); 616 | #endif 617 | PARSE_DOUBLE_MIN_MAX(v_nominal, 7.0, 18.0) 618 | #ifdef DEBUG 619 | fprintf(stderr, "v_nominal: %f\n", settings.v_nominal); 620 | #endif 621 | PARSE_BOOL(enable_magnetometer) 622 | 623 | // FLIGHT MODES 624 | PARSE_INT_MIN_MAX(num_dsm_modes, 1, 3) 625 | if (json_object_object_get_ex(jobj, "flight_mode_1", &tmp) == 0) 626 | { 627 | fprintf(stderr, "ERROR: can't find flight_mode_1 in settings file\n"); 628 | return -1; 629 | } 630 | if (__parse_flight_mode(tmp, &settings.flight_mode_1)) return -1; 631 | #ifdef DEBUG 632 | fprintf(stderr, "flight_mode_1: %d\n", settings.flight_mode_1); 633 | #endif 634 | if (json_object_object_get_ex(jobj, "flight_mode_2", &tmp) == 0) 635 | { 636 | fprintf(stderr, "ERROR: can't find flight_mode_2 in settings file\n"); 637 | return -1; 638 | } 639 | if (__parse_flight_mode(tmp, &settings.flight_mode_2)) return -1; 640 | #ifdef DEBUG 641 | fprintf(stderr, "flight_mode_2: %d\n", settings.flight_mode_2); 642 | #endif 643 | if (json_object_object_get_ex(jobj, "flight_mode_3", &tmp) == 0) 644 | { 645 | fprintf(stderr, "ERROR: can't find flight_mode_3 in settings file\n"); 646 | return -1; 647 | } 648 | if (__parse_flight_mode(tmp, &settings.flight_mode_3)) return -1; 649 | #ifdef DEBUG 650 | fprintf(stderr, "flight_mode_3: %d\n", settings.flight_mode_3); 651 | #endif 652 | 653 | // DSM RADIO CONFIG 654 | PARSE_INT_MIN_MAX(dsm_thr_ch, 1, 9) 655 | PARSE_POLARITY(dsm_thr_pol) 656 | PARSE_INT_MIN_MAX(dsm_roll_ch, 1, 9) 657 | PARSE_POLARITY(dsm_roll_pol) 658 | PARSE_INT_MIN_MAX(dsm_pitch_ch, 1, 9) 659 | PARSE_POLARITY(dsm_pitch_pol) 660 | PARSE_INT_MIN_MAX(dsm_yaw_ch, 1, 9) 661 | PARSE_POLARITY(dsm_yaw_pol) 662 | PARSE_INT_MIN_MAX(dsm_mode_ch, 1, 9) 663 | PARSE_POLARITY(dsm_mode_pol) 664 | if (__parse_kill_mode() == -1) return -1; 665 | #ifdef DEBUG 666 | fprintf(stderr, "kill_mode: %d\n", settings.dsm_kill_mode); 667 | #endif 668 | PARSE_INT_MIN_MAX(dsm_kill_ch, 1, 9) 669 | PARSE_POLARITY(dsm_kill_pol) 670 | 671 | // PRINTF OPTIONS 672 | PARSE_BOOL(printf_arm) 673 | PARSE_BOOL(printf_altitude) 674 | PARSE_BOOL(printf_rpy) 675 | PARSE_BOOL(printf_sticks) 676 | PARSE_BOOL(printf_setpoint) 677 | PARSE_BOOL(printf_u) 678 | PARSE_BOOL(printf_motors) 679 | PARSE_BOOL(printf_mode) 680 | 681 | // LOGGING 682 | PARSE_BOOL(enable_logging) 683 | PARSE_BOOL(log_sensors) 684 | PARSE_BOOL(log_state) 685 | PARSE_BOOL(log_setpoint) 686 | PARSE_BOOL(log_control_u) 687 | PARSE_BOOL(log_motor_signals) 688 | 689 | // MAVLINK 690 | PARSE_STRING(dest_ip) 691 | PARSE_INT(my_sys_id) 692 | PARSE_INT(mav_port) 693 | 694 | // FEEDBACK CONTROLLERS 695 | PARSE_CONTROLLER(roll_controller) 696 | PARSE_CONTROLLER(pitch_controller) 697 | PARSE_CONTROLLER(yaw_controller) 698 | PARSE_CONTROLLER(altitude_controller) 699 | PARSE_CONTROLLER(horiz_vel_ctrl_4dof) 700 | PARSE_CONTROLLER(horiz_vel_ctrl_6dof) 701 | PARSE_CONTROLLER(horiz_pos_ctrl_4dof) 702 | PARSE_CONTROLLER(horiz_pos_ctrl_6dof) 703 | PARSE_DOUBLE_MIN_MAX(max_XY_velocity, .1, 10) 704 | PARSE_DOUBLE_MIN_MAX(max_Z_velocity, .1, 10) 705 | 706 | json_object_put(jobj); // free memory 707 | was_load_successful = 1; 708 | return 0; 709 | } 710 | 711 | int settings_print(void) 712 | { 713 | if (jobj == NULL) 714 | { 715 | fprintf(stderr, "ERROR: NULL object passed to settings_print\n"); 716 | return -1; 717 | } 718 | printf("settings:\n\n"); 719 | printf("%s", 720 | json_object_to_json_string_ext(jobj, JSON_C_TO_STRING_SPACED | JSON_C_TO_STRING_PRETTY)); 721 | printf("\n"); 722 | return 0; 723 | } 724 | -------------------------------------------------------------------------------- /src/state_estimator.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file state_estimator.c 3 | * 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #define TWO_PI (M_PI * 2.0) 25 | 26 | state_estimate_t state_estimate; // extern variable in state_estimator.h 27 | 28 | // sensor data structs 29 | rc_mpu_data_t mpu_data; 30 | static rc_bmp_data_t bmp_data; 31 | 32 | // battery filter 33 | static rc_filter_t batt_lp = RC_FILTER_INITIALIZER; 34 | 35 | // altitude filter components 36 | static rc_kalman_t alt_kf = RC_KALMAN_INITIALIZER; 37 | static rc_filter_t acc_lp = RC_FILTER_INITIALIZER; 38 | 39 | static void __batt_init(void) 40 | { 41 | // init the battery low pass filter 42 | rc_filter_moving_average(&batt_lp, 20, DT); 43 | double tmp = rc_adc_dc_jack(); 44 | if (tmp < 3.0) 45 | { 46 | tmp = settings.v_nominal; 47 | if (settings.warnings_en) 48 | { 49 | fprintf(stderr, "WARNING: ADC read %0.1fV on the barrel jack. Please connect\n"); 50 | fprintf(stderr, "battery to barrel jack, assuming nominal voltage for now.\n"); 51 | } 52 | } 53 | rc_filter_prefill_inputs(&batt_lp, tmp); 54 | rc_filter_prefill_outputs(&batt_lp, tmp); 55 | return; 56 | } 57 | 58 | static void __batt_march(void) 59 | { 60 | double tmp = rc_adc_dc_jack(); 61 | if (tmp < 3.0) tmp = settings.v_nominal; 62 | state_estimate.v_batt_raw = tmp; 63 | state_estimate.v_batt_lp = rc_filter_march(&batt_lp, tmp); 64 | return; 65 | } 66 | 67 | static void __batt_cleanup(void) 68 | { 69 | rc_filter_free(&batt_lp); 70 | return; 71 | } 72 | 73 | static void __imu_march(void) 74 | { 75 | static double last_yaw = 0.0; 76 | static int num_yaw_spins = 0; 77 | double diff; 78 | 79 | // gyro and accel require converting to NED coordinates 80 | state_estimate.gyro[0] = mpu_data.gyro[1]; 81 | state_estimate.gyro[1] = mpu_data.gyro[0]; 82 | state_estimate.gyro[2] = -mpu_data.gyro[2]; 83 | state_estimate.accel[0] = mpu_data.accel[1]; 84 | state_estimate.accel[1] = mpu_data.accel[0]; 85 | state_estimate.accel[2] = -mpu_data.accel[2]; 86 | 87 | // quaternion also needs coordinate transform 88 | state_estimate.quat_imu[0] = mpu_data.dmp_quat[0]; // W 89 | state_estimate.quat_imu[1] = mpu_data.dmp_quat[2]; // X (i) 90 | state_estimate.quat_imu[2] = mpu_data.dmp_quat[1]; // Y (j) 91 | state_estimate.quat_imu[3] = -mpu_data.dmp_quat[3]; // Z (k) 92 | 93 | // normalize it just in case 94 | rc_quaternion_norm_array(state_estimate.quat_imu); 95 | // generate tait bryan angles 96 | rc_quaternion_to_tb_array(state_estimate.quat_imu, state_estimate.tb_imu); 97 | 98 | // yaw is more annoying since we have to detect spins 99 | // also make sign negative since NED coordinates has Z point down 100 | diff = state_estimate.tb_imu[2] + (num_yaw_spins * TWO_PI) - last_yaw; 101 | // detect the crossover point at +-PI and update num yaw spins 102 | if (diff < -M_PI) 103 | num_yaw_spins++; 104 | else if (diff > M_PI) 105 | num_yaw_spins--; 106 | 107 | // finally the new value can be written 108 | state_estimate.imu_continuous_yaw = state_estimate.tb_imu[2] + (num_yaw_spins * TWO_PI); 109 | last_yaw = state_estimate.imu_continuous_yaw; 110 | return; 111 | } 112 | 113 | static void __mag_march(void) 114 | { 115 | static double last_yaw = 0.0; 116 | static int num_yaw_spins = 0; 117 | 118 | // don't do anything if mag isn't enabled 119 | if (!settings.enable_magnetometer) return; 120 | 121 | // mag require converting to NED coordinates 122 | state_estimate.mag[0] = mpu_data.mag[1]; 123 | state_estimate.mag[1] = mpu_data.mag[0]; 124 | state_estimate.mag[2] = -mpu_data.mag[2]; 125 | 126 | // quaternion also needs coordinate transform 127 | state_estimate.quat_mag[0] = mpu_data.fused_quat[0]; // W 128 | state_estimate.quat_mag[1] = mpu_data.fused_quat[2]; // X (i) 129 | state_estimate.quat_mag[2] = mpu_data.fused_quat[1]; // Y (j) 130 | state_estimate.quat_mag[3] = -mpu_data.fused_quat[3]; // Z (k) 131 | 132 | // normalize it just in case 133 | rc_quaternion_norm_array(state_estimate.quat_mag); 134 | // generate tait bryan angles 135 | rc_quaternion_to_tb_array(state_estimate.quat_mag, state_estimate.tb_mag); 136 | 137 | // heading 138 | state_estimate.mag_heading_raw = mpu_data.compass_heading_raw; 139 | state_estimate.mag_heading = state_estimate.tb_mag[2]; 140 | 141 | // yaw is more annoying since we have to detect spins 142 | // also make sign negative since NED coordinates has Z point down 143 | double diff = state_estimate.tb_mag[2] + (num_yaw_spins * TWO_PI) - last_yaw; 144 | // detect the crossover point at +-PI and update num yaw spins 145 | if (diff < -M_PI) 146 | num_yaw_spins++; 147 | else if (diff > M_PI) 148 | num_yaw_spins--; 149 | 150 | // finally the new value can be written 151 | state_estimate.mag_heading_continuous = state_estimate.tb_mag[2] + (num_yaw_spins * TWO_PI); 152 | last_yaw = state_estimate.mag_heading_continuous; 153 | return; 154 | } 155 | 156 | /** 157 | * @brief initialize the altitude kalman filter 158 | * 159 | * @return 0 on success, -1 on failure 160 | */ 161 | static int __altitude_init(void) 162 | { 163 | // initialize altitude kalman filter and bmp sensor 164 | rc_matrix_t F = RC_MATRIX_INITIALIZER; 165 | rc_matrix_t G = RC_MATRIX_INITIALIZER; 166 | rc_matrix_t H = RC_MATRIX_INITIALIZER; 167 | rc_matrix_t Q = RC_MATRIX_INITIALIZER; 168 | rc_matrix_t R = RC_MATRIX_INITIALIZER; 169 | rc_matrix_t Pi = RC_MATRIX_INITIALIZER; 170 | 171 | const int Nx = 3; 172 | const int Ny = 1; 173 | const int Nu = 1; 174 | 175 | // allocate appropirate memory for system 176 | rc_matrix_zeros(&F, Nx, Nx); 177 | rc_matrix_zeros(&G, Nx, Nu); 178 | rc_matrix_zeros(&H, Ny, Nx); 179 | rc_matrix_zeros(&Q, Nx, Nx); 180 | rc_matrix_zeros(&R, Ny, Ny); 181 | rc_matrix_zeros(&Pi, Nx, Nx); 182 | 183 | // define system -DT; // accel bias 184 | F.d[0][0] = 1.0; 185 | F.d[0][1] = DT; 186 | F.d[0][2] = 0.0; 187 | F.d[1][0] = 0.0; 188 | F.d[1][1] = 1.0; 189 | F.d[1][2] = -DT; // subtract accel bias 190 | F.d[2][0] = 0.0; 191 | F.d[2][1] = 0.0; 192 | F.d[2][2] = 1.0; // accel bias state 193 | 194 | G.d[0][0] = 0.5 * DT * DT; 195 | G.d[0][1] = DT; 196 | G.d[0][2] = 0.0; 197 | 198 | H.d[0][0] = 1.0; 199 | H.d[0][1] = 0.0; 200 | H.d[0][2] = 0.0; 201 | 202 | // covariance matrices 203 | Q.d[0][0] = 0.000000001; 204 | Q.d[1][1] = 0.000000001; 205 | Q.d[2][2] = 0.0001; // don't want bias to change too quickly 206 | R.d[0][0] = 1000000.0; 207 | 208 | // initial P, cloned from converged P while running 209 | Pi.d[0][0] = 1258.69; 210 | Pi.d[0][1] = 158.6114; 211 | Pi.d[0][2] = -9.9937; 212 | Pi.d[1][0] = 158.6114; 213 | Pi.d[1][1] = 29.9870; 214 | Pi.d[1][2] = -2.5191; 215 | Pi.d[2][0] = -9.9937; 216 | Pi.d[2][1] = -2.5191; 217 | Pi.d[2][2] = 0.3174; 218 | 219 | // initialize the kalman filter 220 | if (rc_kalman_alloc_lin(&alt_kf, F, G, H, Q, R, Pi) == -1) return -1; 221 | rc_matrix_free(&F); 222 | rc_matrix_free(&G); 223 | rc_matrix_free(&H); 224 | rc_matrix_free(&Q); 225 | rc_matrix_free(&R); 226 | rc_matrix_free(&Pi); 227 | 228 | // initialize the little LP filter to take out accel noise 229 | if (rc_filter_first_order_lowpass(&acc_lp, DT, 20 * DT)) return -1; 230 | 231 | // init barometer and read in first data 232 | if (rc_bmp_read(&bmp_data)) return -1; 233 | 234 | return 0; 235 | } 236 | 237 | static void __altitude_march(void) 238 | { 239 | int i; 240 | double accel_vec[3]; 241 | static rc_vector_t u = RC_VECTOR_INITIALIZER; 242 | static rc_vector_t y = RC_VECTOR_INITIALIZER; 243 | 244 | // grab raw data 245 | state_estimate.bmp_pressure_raw = bmp_data.pressure_pa; 246 | state_estimate.alt_bmp_raw = bmp_data.alt_m; 247 | state_estimate.bmp_temp = bmp_data.temp_c; 248 | 249 | // make copy of acceleration reading before rotating 250 | for (i = 0; i < 3; i++) accel_vec[i] = state_estimate.accel[i]; 251 | 252 | // rotate accel vector 253 | rc_quaternion_rotate_vector_array(accel_vec, state_estimate.quat_imu); 254 | 255 | // do first-run filter setup 256 | if (alt_kf.step == 0) 257 | { 258 | rc_vector_zeros(&u, 1); 259 | rc_vector_zeros(&y, 1); 260 | alt_kf.x_est.d[0] = -bmp_data.alt_m; 261 | rc_filter_prefill_inputs(&acc_lp, accel_vec[2] + GRAVITY); 262 | rc_filter_prefill_outputs(&acc_lp, accel_vec[2] + GRAVITY); 263 | } 264 | 265 | // calculate acceleration and smooth it just a tad 266 | // put result in u for kalman and flip sign since with altitude, positive 267 | // is up whereas acceleration in Z points down. 268 | rc_filter_march(&acc_lp, accel_vec[2] + GRAVITY); 269 | u.d[0] = acc_lp.newest_output; 270 | 271 | // don't bother filtering Barometer, kalman will deal with that 272 | y.d[0] = -bmp_data.alt_m; 273 | 274 | rc_kalman_update_lin(&alt_kf, u, y); 275 | 276 | // altitude estimate 277 | state_estimate.alt_bmp = alt_kf.x_est.d[0]; 278 | state_estimate.alt_bmp_vel = alt_kf.x_est.d[1]; 279 | state_estimate.alt_bmp_accel = alt_kf.x_est.d[2]; 280 | 281 | return; 282 | } 283 | 284 | static void __feedback_select(void) 285 | { 286 | state_estimate.roll = state_estimate.tb_imu[0]; 287 | state_estimate.pitch = state_estimate.tb_imu[1]; 288 | state_estimate.yaw = state_estimate.tb_imu[2]; 289 | state_estimate.continuous_yaw = state_estimate.imu_continuous_yaw; 290 | state_estimate.X = state_estimate.pos_mocap[0]; 291 | state_estimate.Y = state_estimate.pos_mocap[1]; 292 | state_estimate.Z = state_estimate.alt_bmp; 293 | } 294 | 295 | static void __altitude_cleanup(void) 296 | { 297 | rc_kalman_free(&alt_kf); 298 | rc_filter_free(&acc_lp); 299 | return; 300 | } 301 | 302 | static void __mocap_check_timeout(void) 303 | { 304 | if (state_estimate.mocap_running) 305 | { 306 | uint64_t current_time = rc_nanos_since_boot(); 307 | // check if mocap data is > 3 steps old 308 | if ((current_time - state_estimate.mocap_timestamp_ns) > (3 * 1E7)) 309 | { 310 | state_estimate.mocap_running = 0; 311 | if (settings.warnings_en) 312 | { 313 | fprintf(stderr, "WARNING, MOCAP LOST VISUAL\n"); 314 | } 315 | } 316 | } 317 | return; 318 | } 319 | 320 | int state_estimator_init(void) 321 | { 322 | __batt_init(); 323 | if (__altitude_init()) return -1; 324 | state_estimate.initialized = 1; 325 | return 0; 326 | } 327 | 328 | int state_estimator_march(void) 329 | { 330 | if (state_estimate.initialized == 0) 331 | { 332 | fprintf(stderr, "ERROR in state_estimator_march, estimator not initialized\n"); 333 | return -1; 334 | } 335 | 336 | // populate state_estimate struct one setion at a time, top to bottom 337 | __batt_march(); 338 | __imu_march(); 339 | __mag_march(); 340 | __altitude_march(); 341 | __feedback_select(); 342 | __mocap_check_timeout(); 343 | return 0; 344 | } 345 | 346 | int state_estimator_jobs_after_feedback(void) 347 | { 348 | static int bmp_sample_counter = 0; 349 | 350 | // check if we need to sample BMP this loop 351 | if (bmp_sample_counter >= BMP_RATE_DIV) 352 | { 353 | // perform the i2c reads to the sensor, on bad read just try later 354 | if (rc_bmp_read(&bmp_data)) return -1; 355 | bmp_sample_counter = 0; 356 | } 357 | bmp_sample_counter++; 358 | return 0; 359 | } 360 | 361 | int state_estimator_cleanup(void) 362 | { 363 | __batt_cleanup(); 364 | __altitude_cleanup(); 365 | return 0; 366 | } -------------------------------------------------------------------------------- /src/thrust_map.c: -------------------------------------------------------------------------------- 1 | /* 2 | * @file thrust_map.c 3 | * 4 | * Most ESC/motor/propeller combinations provide a highly non-linear map from 5 | * input to thrust. For the thrust table defined in thrust_map.h, this provides 6 | * the function to translate a desired normalized thrust (0-1) to the necessary 7 | * input (also 0-1). 8 | **/ 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | static double* signal; 16 | static double* thrust; 17 | static int points; 18 | 19 | // clang-format off 20 | 21 | // Generic linear mapping 22 | static const int linear_map_points = 11; 23 | static double linear_map[][2] = { 24 | {0.0, 0.0000}, 25 | {0.1, 0.1000}, 26 | {0.2, 0.2000}, 27 | {0.3, 0.3000}, 28 | {0.4, 0.4000}, 29 | {0.5, 0.5000}, 30 | {0.6, 0.6000}, 31 | {0.7, 0.7000}, 32 | {0.8, 0.8000}, 33 | {0.9, 0.9000}, 34 | {1.0, 1.0000}}; 35 | 36 | // Tiger Motor MN1806, 1400KV 6x4.5" 3-blade prop, 14.8V, 37 | // BLheli ESC Low Timing 38 | // this one is in Newtons but it doesn't really matter 39 | static const int mn1806_1400kv_4s_points = 11; 40 | static double mn1806_1400kv_4s_map[][2] = { 41 | {0.0, 0.0000}, 42 | {0.1, 0.2982}, 43 | {0.2, 0.6310}, 44 | {0.3, 1.0281}, 45 | {0.4, 1.5224}, 46 | {0.5, 2.0310}, 47 | {0.6, 2.5791}, 48 | {0.7, 3.1365}, 49 | {0.8, 3.7282}, 50 | {0.9, 4.3147}, 51 | {1.0, 4.7258}}; 52 | 53 | // tiger motor F20 2300kv motor, 2S lipo, 4x4.0" 3-blade props 54 | // blheli esc med-low timing 55 | // thrust units in gram-force but doesn't really matter 56 | static const int f20_2300kv_2s_points = 21; 57 | static double f20_2300kv_2s_map[][2] = { 58 | {0.00, 0.000000}, 59 | {0.05, 6.892067}, 60 | {0.10, 12.57954}, 61 | {0.15, 18.84790}, 62 | {0.20, 26.16294}, 63 | {0.25, 33.98255}, 64 | {0.30, 41.60790}, 65 | {0.35, 49.32732}, 66 | {0.40, 58.27048}, 67 | {0.45, 67.83613}, 68 | {0.50, 78.20817}, 69 | {0.55, 88.27728}, 70 | {0.60, 100.1058}, 71 | {0.65, 110.3643}, 72 | {0.70, 121.6316}, 73 | {0.75, 132.2155}, 74 | {0.80, 145.0420}, 75 | {0.85, 154.6838}, 76 | {0.90, 162.0185}, 77 | {0.95, 168.4321}, 78 | {1.00, 177.1643}}; 79 | 80 | /* 81 | * Lumenier RX2206-13 2000kv motor, 4S lipo, 5x45" lumenier prop 82 | * blheli esc high timing 83 | * for 5" monocoque hex 84 | */ 85 | static const int rx2206_4s_points = 12; 86 | static double rx2206_4s_map[][2] = { 87 | {0.0, 0.00000000000000}, 88 | {0.05, 17.8844719758775}, 89 | {0.145, 44.8761484808831}, 90 | {0.24, 80.0271164157384}, 91 | {0.335, 122.556484678150}, 92 | {0.43, 168.358712108506}, 93 | {0.525, 220.433636910433}, 94 | {0.62, 277.201919870206}, 95 | {0.715, 339.008615108196}, 96 | {0.81, 418.819295994349}, 97 | {0.905, 505.430124336786}, 98 | {1.0, 566.758535098236}}; 99 | 100 | // clang-format on 101 | 102 | int thrust_map_init(thrust_map_t map) 103 | { 104 | int i; 105 | double max; 106 | double(*data)[2]; // pointer to constant data 107 | 108 | switch (map) 109 | { 110 | case LINEAR_MAP: 111 | points = linear_map_points; 112 | data = linear_map; 113 | break; 114 | case MN1806_1400KV_4S: 115 | points = mn1806_1400kv_4s_points; 116 | data = mn1806_1400kv_4s_map; 117 | break; 118 | case F20_2300KV_2S: 119 | points = f20_2300kv_2s_points; 120 | data = f20_2300kv_2s_map; 121 | break; 122 | case RX2206_4S: 123 | points = rx2206_4s_points; 124 | data = rx2206_4s_map; 125 | break; 126 | default: 127 | fprintf(stderr, "ERROR: unknown thrust map\n"); 128 | return -1; 129 | } 130 | 131 | // sanity checks 132 | if (points < 2) 133 | { 134 | fprintf(stderr, "ERROR: need at least 2 datapoints in THRUST_MAP\n"); 135 | return -1; 136 | } 137 | if (data[0][0] != 0.0) 138 | { 139 | fprintf(stderr, "ERROR: first row input must be 0.0\n"); 140 | return -1; 141 | } 142 | if (data[points - 1][0] != 1.0) 143 | { 144 | fprintf(stderr, "ERROR: last row input must be 1.0\n"); 145 | printf("data: %f\n", data[points - 1][0]); 146 | return -1; 147 | } 148 | if (data[0][1] != 0.0) 149 | { 150 | fprintf(stderr, "ERROR: first row thrust must be 0.0\n"); 151 | return -1; 152 | } 153 | if (data[points - 1][1] < 0.0) 154 | { 155 | fprintf(stderr, "ERROR: last row thrust must be > 0.0\n"); 156 | return -1; 157 | } 158 | for (i = 1; i < points; i++) 159 | { 160 | if (data[i][0] <= data[i - 1][0] || data[i][1] <= data[i - 1][1]) 161 | { 162 | fprintf(stderr, "ERROR: thrust_map must be monotonically increasing\n"); 163 | return -1; 164 | } 165 | } 166 | 167 | // create new global array of normalized thrust and inputs 168 | if (signal != NULL) free(signal); 169 | if (thrust != NULL) free(thrust); 170 | signal = (double*)malloc(points * sizeof(double)); 171 | thrust = (double*)malloc(points * sizeof(double)); 172 | max = data[points - 1][1]; 173 | for (i = 0; i < points; i++) 174 | { 175 | signal[i] = data[i][0]; 176 | thrust[i] = data[i][1] / max; 177 | } 178 | return 0; 179 | } 180 | 181 | double map_motor_signal(double m) 182 | { 183 | int i; 184 | double pos; 185 | 186 | // sanity check 187 | if (m > 1.0 || m < 0.0) 188 | { 189 | printf("ERROR: desired thrust t must be between 0.0 & 1.0\n"); 190 | return -1; 191 | } 192 | 193 | // return quickly for boundary conditions 194 | if (m == 0.0 || m == 1.0) return m; 195 | 196 | // scan through the data to pick the upper and lower points to interpolate 197 | for (i = 1; i < points; i++) 198 | { 199 | if (m <= thrust[i]) 200 | { 201 | pos = (m - thrust[i - 1]) / (thrust[i] - thrust[i - 1]); 202 | return signal[i - 1] + (pos * (signal[i] - signal[i - 1])); 203 | } 204 | } 205 | 206 | fprintf(stderr, "ERROR: something in map_motor_signal went wrong\n"); 207 | return -1; 208 | } 209 | --------------------------------------------------------------------------------