Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Merge branch 'master' into custom-servo-mixers

Browse source
This commit is contained in:
Dominic Clifton 2015-07-08 12:11:49 +01:00
commit bb0f909e83
26 changed files with 1394 additions and 248 deletions
Download patch file Download diff file Expand all files Collapse all files

3
Makefile
View file

@ -27,7 +27,7 @@ OPBL ?=no
DEBUG ?= DEBUG ?=
# Serial port/Device for flashing # Serial port/Device for flashing
SERIAL_DEVICE ?= /dev/ttyUSB0 SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found)
# Flash size (KB). Some low-end chips actually have more flash than advertised, use this to override. # Flash size (KB). Some low-end chips actually have more flash than advertised, use this to override.
FLASH_SIZE ?= FLASH_SIZE ?=
@ -229,6 +229,7 @@ COMMON_SRC = build_config.c \
flight/imu.c \ flight/imu.c \
flight/mixer.c \ flight/mixer.c \
flight/lowpass.c \ flight/lowpass.c \
flight/filter.c \
drivers/bus_i2c_soft.c \ drivers/bus_i2c_soft.c \
drivers/serial.c \ drivers/serial.c \
drivers/sound_beeper.c \ drivers/sound_beeper.c \

2
docs/Blackbox.md
View file

@ -146,6 +146,8 @@ These chips are also supported:
* Micron/ST M25P16 - 16 Mbit * Micron/ST M25P16 - 16 Mbit
* Micron N25Q064 - 64 Mbit * Micron N25Q064 - 64 Mbit
* Winbond W25Q64 - 64 Mbit * Winbond W25Q64 - 64 Mbit
* Micron N25Q0128 - 128 Mbit
* Winbond W25Q128 - 128 Mbit
## Enabling the Blackbox (CLI) ## Enabling the Blackbox (CLI)
In the [Cleanflight Configurator][] , enter the CLI tab. Enable the Blackbox feature by typing in `feature BLACKBOX` and In the [Cleanflight Configurator][] , enter the CLI tab. Enable the Blackbox feature by typing in `feature BLACKBOX` and

1
docs/Board - Naze32.md
View file

@ -66,3 +66,4 @@ When SOFTSERIAL is enabled, LED_STRIP and CURRENT_METER are unavailable, but two
|O O| |O O|
\-------[USB]-------/ \-------[USB]-------/
``` ```

3
docs/Cli.md
View file

@ -246,6 +246,9 @@ Re-apply any new defaults as desired.
| `p_vel` | | 0 | 200 | 120 | Profile | UINT8 | | `p_vel` | | 0 | 200 | 120 | Profile | UINT8 |
| `i_vel` | | 0 | 200 | 45 | Profile | UINT8 | | `i_vel` | | 0 | 200 | 45 | Profile | UINT8 |
| `d_vel` | | 0 | 200 | 1 | Profile | UINT8 | | `d_vel` | | 0 | 200 | 1 | Profile | UINT8 |
| `dterm_cut_hz` | Lowpass cutoff filter for Dterm for all PID controllers | 0 | 200 | 0 | Profile | UINT8 |
| `pterm_cut_hz` | Lowpass cutoff filter for Pterm for all PID controllers | 0 | 200 | 0 | Profile | UINT8 |
| `gyro_cut_hz` | Lowpass cutoff filter for gyro input | 0 | 200 | 0 | Profile | UINT8 | | 0 | 200 | 0 | Profile | UINT8 |
| `yaw_p_limit` | Limiter for yaw P term. This parameter is only affecting PID controller 3-5. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | | `yaw_p_limit` | Limiter for yaw P term. This parameter is only affecting PID controller 3-5. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 |
| `blackbox_rate_num` | | 1 | 32 | 1 | Master | UINT8 | | `blackbox_rate_num` | | 1 | 32 | 1 | Master | UINT8 |
| `blackbox_rate_denom` | | 1 | 32 | 1 | Master | UINT8 | | `blackbox_rate_denom` | | 1 | 32 | 1 | Master | UINT8 |

1
docs/Controls.md
View file

@ -40,6 +40,7 @@ The stick positions are combined to activate different functions:
| Enable LCD Page Cycling | LOW | CENTER | HIGH | HIGH | | Enable LCD Page Cycling | LOW | CENTER | HIGH | HIGH |
| Save setting | LOW | LOW | LOW | HIGH | | Save setting | LOW | LOW | LOW | HIGH |
![Stick Positions](assets/images/StickPositions.png)
Download a graphic [cheat sheet](https://multiwii.googlecode.com/svn/branches/Hamburger/MultiWii-StickConfiguration-23_v0-5772156649.pdf) with Tx stick commands (the latest version can always be found Download a graphic [cheat sheet](https://multiwii.googlecode.com/svn/branches/Hamburger/MultiWii-StickConfiguration-23_v0-5772156649.pdf) with Tx stick commands (the latest version can always be found
[here](https://code.google.com/p/multiwii/source/browse/#svn%2Fbranches%2FHamburger)). [here](https://code.google.com/p/multiwii/source/browse/#svn%2Fbranches%2FHamburger)).

BIN
docs/assets/images/StickPositions.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 104 KiB

805
docs/assets/images/StickPositions.svg Normal file
View file

@ -0,0 +1,805 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="2478.5964mm"
height="1086.9835mm"
viewBox="0 0 8782.4284 3851.5164"
id="svg2"
version="1.1"
inkscape:version="0.91 r13725"
sodipodi:docname="StickPositions.svg"
inkscape:export-filename="C:\Users\stuphi\Dropbox\projects\quad\StickPositions.png"
inkscape:export-xdpi="74.996788"
inkscape:export-ydpi="74.996788">
<defs
id="defs4" />
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="0.08"
inkscape:cx="2990.2403"
inkscape:cy="3041.654"
inkscape:document-units="px"
inkscape:current-layer="g4157"
showgrid="false"
inkscape:window-width="1920"
inkscape:window-height="1137"
inkscape:window-x="-8"
inkscape:window-y="-8"
inkscape:window-maximized="1"
fit-margin-top="1"
fit-margin-left="1"
fit-margin-right="1"
fit-margin-bottom="1"
units="mm"
showborder="true" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Frame"
inkscape:groupmode="layer"
id="layer1"
transform="translate(2203.3186,1653.7717)"
style="display:inline" />
<g
inkscape:groupmode="layer"
id="layer2"
inkscape:label="StickCentre"
style="display:inline"
transform="translate(2203.3186,1962.0394)" />
<g
inkscape:groupmode="layer"
id="layer3"
inkscape:label="StickDown"
style="display:inline"
transform="translate(2203.3186,1962.0394)" />
<g
style="display:inline"
inkscape:label="StickDownLeft"
id="g4169"
inkscape:groupmode="layer"
transform="translate(2203.3186,1962.0394)" />
<g
inkscape:groupmode="layer"
id="g4173"
inkscape:label="StickDownRight"
style="display:inline"
transform="translate(2203.3186,1962.0394)" />
<g
style="display:inline"
inkscape:label="StickUpRight"
id="g4177"
inkscape:groupmode="layer"
transform="translate(2203.3186,1962.0394)" />
<g
inkscape:groupmode="layer"
id="g4181"
inkscape:label="StickUpLeft"
style="display:inline"
transform="translate(2203.3186,1962.0394)" />
<g
style="display:inline"
inkscape:label="StickRight"
id="g4161"
inkscape:groupmode="layer"
transform="translate(2203.3186,1962.0394)" />
<g
inkscape:groupmode="layer"
id="g4165"
inkscape:label="StickLeft"
style="display:inline"
transform="translate(2203.3186,1962.0394)" />
<g
inkscape:label="StickUp"
id="g4157"
inkscape:groupmode="layer"
style="display:inline"
transform="translate(2203.3186,1962.0394)">
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4136"
width="288.94339"
height="288.94339"
x="708.38544"
y="-1390.3773"
ry="0" />
<path
inkscape:transform-center-x="-73.935657"
inkscape:transform-center-y="73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 948.22718,-1104.1681 c 7.47613,7.4761 23.9146,3.1588 36.71634,-9.6429 12.80173,-12.8017 17.11898,-29.2402 9.64285,-36.7163 -7.47612,-7.4762 -126.05745,-105.3018 -138.85919,-92.5 -12.80175,12.8017 85.02386,131.383 92.5,138.8592 z"
id="path4175"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
ry="0"
y="-1390.3773"
x="1114.0997"
height="288.94339"
width="288.94339"
id="rect4208"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<circle
r="31.819805"
cy="-1245.9056"
cx="1258.5714"
id="circle4210"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:10;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="486.66803"
y="-1140.8967"
id="text4574"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="486.668"
y="-1140.8967"
id="tspan4578"
style="text-align:end;text-anchor:end">Arm</tspan></text>
<rect
ry="0"
y="-969.96918"
x="708.38544"
height="288.94339"
width="288.94339"
id="rect4615"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
inkscape:transform-center-x="73.935653"
inkscape:transform-center-y="73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 756.60309,-683.75996 c -7.47613,7.47613 -23.9146,3.15888 -36.71634,-9.64286 -12.80173,-12.80174 -17.11899,-29.2402 -9.64285,-36.71634 7.47612,-7.47614 126.05745,-105.30174 138.85919,-92.5 12.80175,12.80174 -85.02386,131.38307 -92.5,138.8592 z"
id="path4621"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4637"
width="288.94339"
height="288.94339"
x="1114.0997"
y="-969.96918"
ry="0" />
<circle
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:10;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="circle4639"
cx="1258.5714"
cy="-825.4975"
r="31.819805" />
<text
sodipodi:linespacing="125%"
id="text4657"
y="-720.48853"
x="486.66803"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan4659"
y="-720.48853"
x="486.66803"
sodipodi:role="line">Disarm</tspan></text>
<rect
ry="0"
y="-549.56104"
x="708.38544"
height="288.94339"
width="288.94339"
id="rect4667"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
inkscape:transform-center-x="73.935653"
inkscape:transform-center-y="73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 756.60309,-263.35179 c -7.47613,7.47613 -23.9146,3.15888 -36.71634,-9.64286 -12.80173,-12.80174 -17.11899,-29.2402 -9.64285,-36.71634 7.47612,-7.47614 126.05745,-105.30174 138.85919,-92.5 12.80175,12.80174 -85.02386,131.38307 -92.5,138.8592 z"
id="path4673"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4689"
width="288.94339"
height="288.94339"
x="1114.0997"
y="-549.56104"
ry="0" />
<path
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 1090.881,-372.67398 c -10.5728,0 -19.1438,-14.67651 -19.1438,-32.7809 0,-18.10439 8.571,-32.7809 19.1438,-32.7809 10.5729,-1e-5 163.5957,14.6765 163.5957,32.7809 0,18.1044 -153.0228,32.78091 -163.5957,32.7809 z"
id="path4705"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss"
inkscape:transform-center-x="94.95434"
inkscape:transform-center-y="0.5050695" />
<text
sodipodi:linespacing="125%"
id="text4709"
y="-300.08038"
x="486.66803"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan4711"
y="-300.08038"
x="486.668"
sodipodi:role="line">Profile 1</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4719"
width="288.94339"
height="288.94339"
x="708.38544"
y="-129.15286"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path4725"
d="m 756.60309,157.05638 c -7.47613,7.47613 -23.9146,3.15888 -36.71634,-9.64286 -12.80173,-12.80174 -17.11899,-29.2402 -9.64285,-36.71634 7.47612,-7.47614 126.05745,-105.30174 138.85919,-92.5 12.80175,12.80174 -85.02386,131.38307 -92.5,138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="73.935653" />
<rect
ry="0"
y="-129.15286"
x="1114.0997"
height="288.94339"
width="288.94339"
id="rect4741"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 1291.3523,-149.39445 c 0,-10.57285 -14.6765,-19.14384 -32.7809,-19.14384 -18.1043,0 -32.7809,8.57099 -32.7809,19.14384 0,10.57285 14.6765,163.59566 32.7809,163.59566 18.1044,0 32.781,-153.02281 32.7809,-163.59566 z"
id="path4759"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="486.66803"
y="120.3278"
id="text4761"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="486.668"
y="120.3278"
id="tspan4763"
style="text-align:end;text-anchor:end">Profile 2</tspan></text>
<rect
ry="0"
y="291.25531"
x="708.38544"
height="288.94339"
width="288.94339"
id="rect4771"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
inkscape:transform-center-x="73.935653"
inkscape:transform-center-y="73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 756.60309,577.46454 c -7.47613,7.47613 -23.9146,3.15888 -36.71634,-9.64286 -12.80173,-12.80174 -17.11899,-29.2402 -9.64285,-36.71634 7.47612,-7.47614 126.05745,-105.30174 138.85919,-92.5 12.80175,12.80174 -85.02386,131.38307 -92.5,138.8592 z"
id="path4777"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4793"
width="288.94339"
height="288.94339"
x="1114.0997"
y="291.25531"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path4807"
d="m 1425.2415,403.08564 c 10.5729,0 19.1439,14.67651 19.1439,32.7809 0,18.10439 -8.571,32.7809 -19.1439,32.7809 -10.5728,10e-6 -163.5957,-14.6765 -163.5957,-32.7809 0,-18.1044 153.0229,-32.78091 163.5957,-32.7809 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
sodipodi:linespacing="125%"
id="text4813"
y="540.73602"
x="486.66803"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan4815"
y="540.73602"
x="486.668"
sodipodi:role="line">Profile 3</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4823"
width="288.94339"
height="288.94339"
x="708.38544"
y="711.66351"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path4829"
d="m 756.60309,997.87274 c -7.47613,7.47616 -23.9146,3.15886 -36.71634,-9.64286 -12.80173,-12.80174 -17.11899,-29.2402 -9.64285,-36.71634 7.47612,-7.47614 126.05745,-105.30174 138.85919,-92.5 12.80175,12.80174 -85.02386,131.38307 -92.5,138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="73.935653" />
<rect
ry="0"
y="711.66351"
x="1114.0997"
height="288.94339"
width="288.94339"
id="rect4845"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path4849"
d="m 1291.3523,1021.9347 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1043,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.5957 32.7809,-163.5957 18.1044,0 32.781,153.0228 32.7809,163.5957 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="486.66803"
y="961.14417"
id="text4865"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="486.66791"
y="961.14417"
id="tspan4867"
style="text-align:end;text-anchor:end">Calibrate Gyro</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4875"
width="288.94339"
height="288.94339"
x="708.38544"
y="1132.0717"
ry="0" />
<path
inkscape:transform-center-x="73.935669"
inkscape:transform-center-y="-73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 757.10817,1133.9385 c -7.47613,-7.4762 -23.9146,-3.1589 -36.71634,9.6428 -12.80173,12.8018 -17.11899,29.2402 -9.64285,36.7164 7.47612,7.4761 126.05745,105.3017 138.85919,92.5 12.80175,-12.8018 -85.02386,-131.3831 -92.5,-138.8592 z"
id="path4887"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
ry="0"
y="1132.0717"
x="1114.0997"
height="288.94339"
width="288.94339"
id="rect4897"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path4901"
d="m 1291.3523,1442.3429 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1043,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.5957 32.7809,-163.5957 18.1044,0 32.781,153.0228 32.7809,163.5957 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="486.66803"
y="1381.5524"
id="text4917"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="486.66803"
y="1381.5524"
id="tspan4919"
style="text-align:end;text-anchor:end">Calibrate Acc</tspan></text>
<rect
ry="0"
y="1552.4799"
x="708.38544"
height="288.94339"
width="288.94339"
id="rect4927"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
inkscape:transform-center-x="-73.935665"
inkscape:transform-center-y="-73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 948.22718,1554.3467 c 7.47613,-7.4762 23.9146,-3.1589 36.71634,9.6428 12.80173,12.8018 17.11898,29.2402 9.64285,36.7164 -7.47612,7.4761 -126.05745,105.3017 -138.85919,92.5 -12.80175,-12.8018 85.02386,-131.3831 92.5,-138.8592 z"
id="path4937"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect4949"
width="288.94339"
height="288.94339"
x="1114.0997"
y="1552.4799"
ry="0" />
<path
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 1291.3523,1862.7511 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1043,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.5957 32.7809,-163.5957 18.1044,0 32.781,153.0228 32.7809,163.5957 z"
id="path4953"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<text
sodipodi:linespacing="125%"
id="text4969"
y="1801.9606"
x="486.66803"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan4971"
y="1801.9606"
x="486.66791"
sodipodi:role="line">Calibrate Compass</tspan></text>
<rect
ry="0"
y="-1390.3773"
x="5835.5283"
height="288.94339"
width="288.94339"
id="rect5607"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
inkscape:transform-center-x="73.935653"
inkscape:transform-center-y="73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 5883.7461,-1104.1681 c -7.4762,7.4761 -23.9146,3.1588 -36.7164,-9.6429 -12.8017,-12.8017 -17.119,-29.2402 -9.6428,-36.7163 7.4761,-7.4762 126.0574,-105.3018 138.8592,-92.5 12.8017,12.8017 -85.0239,131.383 -92.5,138.8592 z"
id="path5613"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5629"
width="288.94339"
height="288.94339"
x="6241.2427"
y="-1390.3773"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5639"
d="m 6481.0845,-1388.5106 c 7.4761,-7.4762 23.9146,-3.1589 36.7163,9.6428 12.8017,12.8018 17.119,29.2402 9.6428,36.7164 -7.4761,7.4761 -126.0574,105.3017 -138.8591,92.5 -12.8018,-12.8018 85.0238,-131.3831 92.5,-138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="-73.935654"
inkscape:transform-center-x="-73.935665" />
<text
sodipodi:linespacing="125%"
id="text5649"
y="-1140.8967"
x="5613.811"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan5651"
y="-1140.8967"
x="5613.811"
sodipodi:role="line">In-flight Calibration Controls</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5659"
width="288.94339"
height="288.94339"
x="5835.5283"
y="-969.96918"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5677"
d="m 6012.781,-990.21079 c 0,-10.57281 -14.6765,-19.14381 -32.7809,-19.14381 -18.1044,0 -32.7809,8.571 -32.7809,19.14381 0,10.57285 14.6765,163.59566 32.7809,163.59566 18.1044,0 32.7809,-153.02281 32.7809,-163.59566 z"
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
ry="0"
y="-969.96918"
x="6241.2427"
height="288.94339"
width="288.94339"
id="rect5681"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
inkscape:transform-center-y="0.5050695"
inkscape:transform-center-x="94.95434"
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5697"
d="m 6218.024,-793.08215 c -10.5729,0 -19.1438,-14.67651 -19.1438,-32.7809 0,-18.10439 8.5709,-32.7809 19.1438,-32.7809 10.5728,-10e-6 163.5957,14.6765 163.5957,32.7809 0,18.1044 -153.0229,32.78091 -163.5957,32.7809 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="5613.811"
y="-720.48853"
id="text5701"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="5613.811"
y="-720.48853"
id="tspan5703"
style="text-align:end;text-anchor:end">Trim Acc Left</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5711"
width="288.94339"
height="288.94339"
x="5835.5283"
y="-549.56104"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5729"
d="m 6012.781,-569.80262 c 0,-10.57285 -14.6765,-19.14384 -32.7809,-19.14384 -18.1044,0 -32.7809,8.57099 -32.7809,19.14384 0,10.57285 14.6765,163.59566 32.7809,163.59566 18.1044,0 32.7809,-153.02281 32.7809,-163.59566 z"
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
ry="0"
y="-549.56104"
x="6241.2427"
height="288.94339"
width="288.94339"
id="rect5733"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 6552.3845,-437.73069 c 10.5728,0 19.1438,14.67651 19.1438,32.7809 0,18.10439 -8.571,32.7809 -19.1438,32.7809 -10.5729,10e-6 -163.5957,-14.6765 -163.5957,-32.7809 0,-18.1044 153.0228,-32.78091 163.5957,-32.7809 z"
id="path5747"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="5613.811"
y="-300.08038"
id="text5753"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="5613.811"
y="-300.08038"
id="tspan5755"
style="text-align:end;text-anchor:end">Trim Acc Right</tspan></text>
<rect
ry="0"
y="-129.15286"
x="5835.5283"
height="288.94339"
width="288.94339"
id="rect5763"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 6012.781,-149.39445 c 0,-10.57285 -14.6765,-19.14384 -32.7809,-19.14384 -18.1044,0 -32.7809,8.57099 -32.7809,19.14384 0,10.57285 14.6765,163.59566 32.7809,163.59566 18.1044,0 32.7809,-153.02281 32.7809,-163.59566 z"
id="path5781"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5785"
width="288.94339"
height="288.94339"
x="6241.2427"
y="-129.15286"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5803"
d="m 6418.4953,-149.39445 c 0,-10.57285 -14.6765,-19.14384 -32.7809,-19.14384 -18.1044,0 -32.7809,8.57099 -32.7809,19.14384 0,10.57285 14.6765,163.59566 32.7809,163.59566 18.1044,0 32.7809,-153.02281 32.7809,-163.59566 z"
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
sodipodi:linespacing="125%"
id="text5805"
y="120.3278"
x="5613.811"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan5807"
y="120.3278"
x="5613.811"
sodipodi:role="line">Trim Acc Forwards</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5815"
width="288.94339"
height="288.94339"
x="5835.5283"
y="291.25531"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5833"
d="m 6012.781,271.01371 c 0,-10.57285 -14.6765,-19.14384 -32.7809,-19.14384 -18.1044,0 -32.7809,8.57099 -32.7809,19.14384 0,10.57285 14.6765,163.59566 32.7809,163.59566 18.1044,0 32.7809,-153.02281 32.7809,-163.59566 z"
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
ry="0"
y="291.25531"
x="6241.2427"
height="288.94339"
width="288.94339"
id="rect5837"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5841"
d="m 6418.4953,601.52646 c 0,10.57285 -14.6765,19.14384 -32.7809,19.14384 -18.1044,0 -32.7809,-8.57099 -32.7809,-19.14384 0,-10.57285 14.6765,-163.59566 32.7809,-163.59566 18.1044,0 32.7809,153.02281 32.7809,163.59566 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="5613.811"
y="540.73602"
id="text5857"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="5613.811"
y="540.73602"
id="tspan5859"
style="text-align:end;text-anchor:end">Trim Acc Backwards</tspan></text>
<rect
ry="0"
y="711.66351"
x="5835.5283"
height="288.94339"
width="288.94339"
id="rect5867"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5871"
d="m 6012.781,1021.9347 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1044,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.5957 32.7809,-163.5957 18.1044,0 32.7809,153.0228 32.7809,163.5957 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5889"
width="288.94339"
height="288.94339"
x="6241.2427"
y="711.66351"
ry="0" />
<path
inkscape:transform-center-x="73.935669"
inkscape:transform-center-y="-73.935654"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 6289.9654,713.53025 c -7.4761,-7.47613 -23.9146,-3.15888 -36.7163,9.64286 -12.8017,12.80174 -17.119,29.2402 -9.6428,36.71634 7.4761,7.47614 126.0574,105.30174 138.8591,92.5 12.8018,-12.80174 -85.0238,-131.38307 -92.5,-138.8592 z"
id="path5901"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
<text
sodipodi:linespacing="125%"
id="text5909"
y="961.14417"
x="5613.811"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan5911"
y="961.14417"
x="5613.811"
sodipodi:role="line">Disable LCD Page Cycling</tspan></text>
<rect
ry="0"
y="1132.0717"
x="5835.5283"
height="288.94339"
width="288.94339"
id="rect5919"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5923"
d="m 6012.781,1442.3429 c 0,10.5728 -14.6765,19.1438 -32.7809,19.1438 -18.1044,0 -32.7809,-8.571 -32.7809,-19.1438 0,-10.5729 14.6765,-163.5957 32.7809,-163.5957 18.1044,0 32.7809,153.0228 32.7809,163.5957 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5941"
width="288.94339"
height="288.94339"
x="6241.2427"
y="1132.0717"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5951"
d="m 6481.0845,1133.9385 c 7.4761,-7.4762 23.9146,-3.1589 36.7163,9.6428 12.8017,12.8018 17.119,29.2402 9.6428,36.7164 -7.4761,7.4761 -126.0574,105.3017 -138.8591,92.5 -12.8018,-12.8018 85.0238,-131.3831 92.5,-138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="-73.935654"
inkscape:transform-center-x="-73.935665" />
<text
sodipodi:linespacing="125%"
id="text5961"
y="1381.5524"
x="5613.811"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
xml:space="preserve"><tspan
style="text-align:end;text-anchor:end"
id="tspan5963"
y="1381.5524"
x="5613.811"
sodipodi:role="line">Enable LCD Page Cycling</tspan></text>
<rect
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
id="rect5971"
width="288.94339"
height="288.94339"
x="5835.5283"
y="1552.4799"
ry="0" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5977"
d="m 5883.7461,1838.6891 c -7.4762,7.4762 -23.9146,3.1589 -36.7164,-9.6428 -12.8017,-12.8018 -17.119,-29.2402 -9.6428,-36.7164 7.4761,-7.4761 126.0574,-105.3017 138.8592,-92.5 12.8017,12.8018 -85.0239,131.3831 -92.5,138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="73.935653" />
<rect
ry="0"
y="1552.4799"
x="6241.2427"
height="288.94339"
width="288.94339"
id="rect5993"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path6001"
d="m 6481.0845,1838.6891 c 7.4761,7.4762 23.9146,3.1589 36.7163,-9.6428 12.8017,-12.8018 17.119,-29.2402 9.6428,-36.7164 -7.4761,-7.4761 -126.0574,-105.3017 -138.8591,-92.5 -12.8018,12.8018 85.0238,131.3831 92.5,138.8592 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="73.935654"
inkscape:transform-center-x="-73.935657" />
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:286.5987854px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="5613.811"
y="1801.9606"
id="text6013"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="5613.811"
y="1801.9606"
id="tspan6015"
style="text-align:end;text-anchor:end">Save Setting</tspan></text>
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:352.85842896px;line-height:125%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="2179.2813"
y="-1690.4064"
id="text7907"
sodipodi:linespacing="125%"><tspan
sodipodi:role="line"
x="2179.2813"
y="-1690.4064"
id="tspan7909"
style="text-align:center;text-anchor:middle">Mode 2 Stick Functions</tspan></text>
</g>
</svg>
Side by side

After

Width:  |  Height:  |  Size: 40 KiB

38
lib/main/STM32_USB-FS-Device_Driver/inc/usb_regs.h
View file

@ -217,26 +217,26 @@ enum EP_BUF_NUM
#define _SetBTABLE(wRegValue)(*BTABLE = (uint16_t)(wRegValue & 0xFFF8)) #define _SetBTABLE(wRegValue)(*BTABLE = (uint16_t)(wRegValue & 0xFFF8))
/* GetCNTR */ /* GetCNTR */
#define _GetCNTR() ((uint16_t) *CNTR) #define _GetCNTR() ((__IO uint16_t) *CNTR)
/* GetISTR */ /* GetISTR */
#define _GetISTR() ((uint16_t) *ISTR) #define _GetISTR() ((__IO uint16_t) *ISTR)
/* GetFNR */ /* GetFNR */
#define _GetFNR() ((uint16_t) *FNR) #define _GetFNR() ((__IO uint16_t) *FNR)
/* GetDADDR */ /* GetDADDR */
#define _GetDADDR() ((uint16_t) *DADDR) #define _GetDADDR() ((__IO uint16_t) *DADDR)
/* GetBTABLE */ /* GetBTABLE */
#define _GetBTABLE() ((uint16_t) *BTABLE) #define _GetBTABLE() ((__IO uint16_t) *BTABLE)
/* SetENDPOINT */ /* SetENDPOINT */
#define _SetENDPOINT(bEpNum,wRegValue) (*(EP0REG + bEpNum)= \ #define _SetENDPOINT(bEpNum,wRegValue) (*(EP0REG + bEpNum)= \
(uint16_t)wRegValue) (uint16_t)wRegValue)
/* GetENDPOINT */ /* GetENDPOINT */
#define _GetENDPOINT(bEpNum) ((uint16_t)(*(EP0REG + bEpNum))) #define _GetENDPOINT(bEpNum) ((__IO uint16_t)(*(EP0REG + bEpNum)))
/******************************************************************************* /*******************************************************************************
* Macro Name : SetEPType * Macro Name : SetEPType
@ -334,9 +334,9 @@ enum EP_BUF_NUM
* Output : None. * Output : None.
* Return : status . * Return : status .
*******************************************************************************/ *******************************************************************************/
#define _GetEPTxStatus(bEpNum) ((uint16_t)_GetENDPOINT(bEpNum) & EPTX_STAT) #define _GetEPTxStatus(bEpNum) ((__IO uint16_t)_GetENDPOINT(bEpNum) & EPTX_STAT)
#define _GetEPRxStatus(bEpNum) ((uint16_t)_GetENDPOINT(bEpNum) & EPRX_STAT) #define _GetEPRxStatus(bEpNum) ((__IO uint16_t)_GetENDPOINT(bEpNum) & EPRX_STAT)
/******************************************************************************* /*******************************************************************************
* Macro Name : SetEPTxValid / SetEPRxValid * Macro Name : SetEPTxValid / SetEPRxValid
@ -446,12 +446,12 @@ enum EP_BUF_NUM
* Output : None. * Output : None.
* Return : None. * Return : None.
*******************************************************************************/ *******************************************************************************/
#define _GetEPAddress(bEpNum) ((uint8_t)(_GetENDPOINT(bEpNum) & EPADDR_FIELD)) #define _GetEPAddress(bEpNum) ((__IO uint8_t)(_GetENDPOINT(bEpNum) & EPADDR_FIELD))
#define _pEPTxAddr(bEpNum) ((uint32_t *)((_GetBTABLE()+bEpNum*8 )*2 + PMAAddr)) #define _pEPTxAddr(bEpNum) ((__IO uint32_t *)((_GetBTABLE()+bEpNum*8 )*2 + PMAAddr))
#define _pEPTxCount(bEpNum) ((uint32_t *)((_GetBTABLE()+bEpNum*8+2)*2 + PMAAddr)) #define _pEPTxCount(bEpNum) ((__IO uint32_t *)((_GetBTABLE()+bEpNum*8+2)*2 + PMAAddr))
#define _pEPRxAddr(bEpNum) ((uint32_t *)((_GetBTABLE()+bEpNum*8+4)*2 + PMAAddr)) #define _pEPRxAddr(bEpNum) ((__IO uint32_t *)((_GetBTABLE()+bEpNum*8+4)*2 + PMAAddr))
#define _pEPRxCount(bEpNum) ((uint32_t *)((_GetBTABLE()+bEpNum*8+6)*2 + PMAAddr)) #define _pEPRxCount(bEpNum) ((__IO uint32_t *)((_GetBTABLE()+bEpNum*8+6)*2 + PMAAddr))
/******************************************************************************* /*******************************************************************************
* Macro Name : SetEPTxAddr / SetEPRxAddr. * Macro Name : SetEPTxAddr / SetEPRxAddr.
@ -471,8 +471,8 @@ enum EP_BUF_NUM
* Output : None. * Output : None.
* Return : address of the buffer. * Return : address of the buffer.
*******************************************************************************/ *******************************************************************************/
#define _GetEPTxAddr(bEpNum) ((uint16_t)*_pEPTxAddr(bEpNum)) #define _GetEPTxAddr(bEpNum) ((__IO uint16_t)*_pEPTxAddr(bEpNum))
#define _GetEPRxAddr(bEpNum) ((uint16_t)*_pEPRxAddr(bEpNum)) #define _GetEPRxAddr(bEpNum) ((__IO uint16_t)*_pEPRxAddr(bEpNum))
/******************************************************************************* /*******************************************************************************
* Macro Name : SetEPCountRxReg. * Macro Name : SetEPCountRxReg.
@ -505,7 +505,7 @@ enum EP_BUF_NUM
#define _SetEPRxDblBuf0Count(bEpNum,wCount) {\ #define _SetEPRxDblBuf0Count(bEpNum,wCount) {\
uint32_t *pdwReg = _pEPTxCount(bEpNum); \ __IO uint32_t *pdwReg = _pEPTxCount(bEpNum); \
_SetEPCountRxReg(pdwReg, wCount);\ _SetEPCountRxReg(pdwReg, wCount);\
} }
/******************************************************************************* /*******************************************************************************
@ -518,7 +518,7 @@ enum EP_BUF_NUM
*******************************************************************************/ *******************************************************************************/
#define _SetEPTxCount(bEpNum,wCount) (*_pEPTxCount(bEpNum) = wCount) #define _SetEPTxCount(bEpNum,wCount) (*_pEPTxCount(bEpNum) = wCount)
#define _SetEPRxCount(bEpNum,wCount) {\ #define _SetEPRxCount(bEpNum,wCount) {\
uint32_t *pdwReg = _pEPRxCount(bEpNum); \ __IO uint32_t *pdwReg = _pEPRxCount(bEpNum); \
_SetEPCountRxReg(pdwReg, wCount);\ _SetEPCountRxReg(pdwReg, wCount);\
} }
/******************************************************************************* /*******************************************************************************
@ -528,8 +528,8 @@ enum EP_BUF_NUM
* Output : None. * Output : None.
* Return : Counter value. * Return : Counter value.
*******************************************************************************/ *******************************************************************************/
#define _GetEPTxCount(bEpNum)((uint16_t)(*_pEPTxCount(bEpNum)) & 0x3ff) #define _GetEPTxCount(bEpNum)((__IO uint16_t)(*_pEPTxCount(bEpNum)) & 0x3ff)
#define _GetEPRxCount(bEpNum)((uint16_t)(*_pEPRxCount(bEpNum)) & 0x3ff) #define _GetEPRxCount(bEpNum)((__IO uint16_t)(*_pEPRxCount(bEpNum)) & 0x3ff)
/******************************************************************************* /*******************************************************************************
* Macro Name : SetEPDblBuf0Addr / SetEPDblBuf1Addr. * Macro Name : SetEPDblBuf0Addr / SetEPDblBuf1Addr.

398
src/main/blackbox/blackbox.c
View file

@ -81,6 +81,7 @@
#define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define SLOW_FRAME_INTERVAL 4096
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
@ -97,35 +98,18 @@
static const char blackboxHeader[] = static const char blackboxHeader[] =
"H Product:Blackbox flight data recorder by Nicholas Sherlock\n" "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
"H Blackbox version:1\n"
"H Data version:2\n" "H Data version:2\n"
"H I interval:" STR(BLACKBOX_I_INTERVAL) "\n"; "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
static const char* const blackboxMainHeaderNames[] = { static const char* const blackboxFieldHeaderNames[] = {
"I name", "name",
"I signed", "signed",
"I predictor", "predictor",
"I encoding", "encoding",
"P predictor", "predictor",
"P encoding" "encoding"
}; };
#ifdef GPS
static const char* const blackboxGPSGHeaderNames[] = {
"G name",
"G signed",
"G predictor",
"G encoding"
};
static const char* const blackboxGPSHHeaderNames[] = {
"H name",
"H signed",
"H predictor",
"H encoding"
};
#endif
/* All field definition structs should look like this (but with longer arrs): */ /* All field definition structs should look like this (but with longer arrs): */
typedef struct blackboxFieldDefinition_t { typedef struct blackboxFieldDefinition_t {
const char *name; const char *name;
@ -136,7 +120,30 @@ typedef struct blackboxFieldDefinition_t {
uint8_t arr[1]; uint8_t arr[1];
} blackboxFieldDefinition_t; } blackboxFieldDefinition_t;
typedef struct blackboxMainFieldDefinition_t { #define BLACKBOX_DELTA_FIELD_HEADER_COUNT ARRAY_LENGTH(blackboxFieldHeaderNames)
#define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
#define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
typedef struct blackboxSimpleFieldDefinition_t {
const char *name;
int8_t fieldNameIndex;
uint8_t isSigned;
uint8_t predict;
uint8_t encode;
} blackboxSimpleFieldDefinition_t;
typedef struct blackboxConditionalFieldDefinition_t {
const char *name;
int8_t fieldNameIndex;
uint8_t isSigned;
uint8_t predict;
uint8_t encode;
uint8_t condition; // Decide whether this field should appear in the log
} blackboxConditionalFieldDefinition_t;
typedef struct blackboxDeltaFieldDefinition_t {
const char *name; const char *name;
int8_t fieldNameIndex; int8_t fieldNameIndex;
@ -146,17 +153,7 @@ typedef struct blackboxMainFieldDefinition_t {
uint8_t Ppredict; uint8_t Ppredict;
uint8_t Pencode; uint8_t Pencode;
uint8_t condition; // Decide whether this field should appear in the log uint8_t condition; // Decide whether this field should appear in the log
} blackboxMainFieldDefinition_t; } blackboxDeltaFieldDefinition_t;
typedef struct blackboxGPSFieldDefinition_t {
const char *name;
int8_t fieldNameIndex;
uint8_t isSigned;
uint8_t predict;
uint8_t encode;
uint8_t condition; // Decide whether this field should appear in the log
} blackboxGPSFieldDefinition_t;
/** /**
* Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
@ -164,7 +161,7 @@ typedef struct blackboxGPSFieldDefinition_t {
* the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
* the encoding we've promised here). * the encoding we've promised here).
*/ */
static const blackboxMainFieldDefinition_t blackboxMainFields[] = { static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
/* loopIteration doesn't appear in P frames since it always increments */ /* loopIteration doesn't appear in P frames since it always increments */
{"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)}, {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
/* Time advances pretty steadily so the P-frame prediction is a straight line */ /* Time advances pretty steadily so the P-frame prediction is a straight line */
@ -226,7 +223,7 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
#ifdef GPS #ifdef GPS
// GPS position/vel frame // GPS position/vel frame
static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = { static const blackboxConditionalFieldDefinition_t blackboxGpsGFields[] = {
{"time", -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)}, {"time", -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
{"GPS_numSat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}, {"GPS_numSat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
{"GPS_coord", 0, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"GPS_coord", 0, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@ -237,28 +234,69 @@ static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = {
}; };
// GPS home frame // GPS home frame
static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = { static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
{"GPS_home", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"GPS_home", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"GPS_home", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)} {"GPS_home", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
}; };
#endif #endif
// Rarely-updated fields
static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}
};
typedef enum BlackboxState { typedef enum BlackboxState {
BLACKBOX_STATE_DISABLED = 0, BLACKBOX_STATE_DISABLED = 0,
BLACKBOX_STATE_STOPPED, BLACKBOX_STATE_STOPPED,
BLACKBOX_STATE_SEND_HEADER, BLACKBOX_STATE_SEND_HEADER,
BLACKBOX_STATE_SEND_FIELDINFO, BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
BLACKBOX_STATE_SEND_GPS_H_HEADERS, BLACKBOX_STATE_SEND_GPS_H_HEADER,
BLACKBOX_STATE_SEND_GPS_G_HEADERS, BLACKBOX_STATE_SEND_GPS_G_HEADER,
BLACKBOX_STATE_SEND_SLOW_HEADER,
BLACKBOX_STATE_SEND_SYSINFO, BLACKBOX_STATE_SEND_SYSINFO,
BLACKBOX_STATE_RUNNING, BLACKBOX_STATE_RUNNING,
BLACKBOX_STATE_SHUTTING_DOWN BLACKBOX_STATE_SHUTTING_DOWN
} BlackboxState; } BlackboxState;
typedef struct gpsState_t { typedef struct blackboxMainState_t {
uint32_t time;
int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT];
int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t accSmooth[XYZ_AXIS_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS];
uint16_t vbatLatest;
uint16_t amperageLatest;
#ifdef BARO
int32_t BaroAlt;
#endif
#ifdef MAG
int16_t magADC[XYZ_AXIS_COUNT];
#endif
#ifdef SONAR
int32_t sonarRaw;
#endif
uint16_t rssi;
} blackboxMainState_t;
typedef struct blackboxGpsState_t {
int32_t GPS_home[2], GPS_coord[2]; int32_t GPS_home[2], GPS_coord[2];
uint8_t GPS_numSat; uint8_t GPS_numSat;
} gpsState_t; } blackboxGpsState_t;
// This data is updated really infrequently:
typedef struct blackboxSlowState_t {
uint16_t flightModeFlags;
uint8_t stateFlags;
uint8_t failsafePhase;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From mixer.c: //From mixer.c:
extern uint8_t motorCount; extern uint8_t motorCount;
@ -292,7 +330,8 @@ static uint32_t blackboxConditionCache;
STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER, too_many_flight_log_conditions); STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER, too_many_flight_log_conditions);
static uint32_t blackboxIteration; static uint32_t blackboxIteration;
static uint32_t blackboxPFrameIndex, blackboxIFrameIndex; static uint16_t blackboxPFrameIndex, blackboxIFrameIndex;
static uint16_t blackboxSlowFrameIterationTimer;
/* /*
* We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated. * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
@ -301,13 +340,14 @@ static uint32_t blackboxPFrameIndex, blackboxIFrameIndex;
*/ */
static uint16_t vbatReference; static uint16_t vbatReference;
static gpsState_t gpsHistory; static blackboxGpsState_t gpsHistory;
static blackboxSlowState_t slowHistory;
// Keep a history of length 2, plus a buffer for MW to store the new values into // Keep a history of length 2, plus a buffer for MW to store the new values into
static blackboxValues_t blackboxHistoryRing[3]; static blackboxMainState_t blackboxHistoryRing[3];
// These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old) // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
static blackboxValues_t* blackboxHistory[3]; static blackboxMainState_t* blackboxHistory[3];
static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
{ {
@ -403,9 +443,10 @@ static void blackboxSetState(BlackboxState newState)
xmitState.headerIndex = 0; xmitState.headerIndex = 0;
xmitState.u.startTime = millis(); xmitState.u.startTime = millis();
break; break;
case BLACKBOX_STATE_SEND_FIELDINFO: case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
case BLACKBOX_STATE_SEND_GPS_G_HEADERS: case BLACKBOX_STATE_SEND_GPS_G_HEADER:
case BLACKBOX_STATE_SEND_GPS_H_HEADERS: case BLACKBOX_STATE_SEND_GPS_H_HEADER:
case BLACKBOX_STATE_SEND_SLOW_HEADER:
xmitState.headerIndex = 0; xmitState.headerIndex = 0;
xmitState.u.fieldIndex = -1; xmitState.u.fieldIndex = -1;
break; break;
@ -416,6 +457,7 @@ static void blackboxSetState(BlackboxState newState)
blackboxIteration = 0; blackboxIteration = 0;
blackboxPFrameIndex = 0; blackboxPFrameIndex = 0;
blackboxIFrameIndex = 0; blackboxIFrameIndex = 0;
blackboxSlowFrameIterationTimer = SLOW_FRAME_INTERVAL; //Force a slow frame to be written on the first iteration
break; break;
case BLACKBOX_STATE_SHUTTING_DOWN: case BLACKBOX_STATE_SHUTTING_DOWN:
xmitState.u.startTime = millis(); xmitState.u.startTime = millis();
@ -429,7 +471,7 @@ static void blackboxSetState(BlackboxState newState)
static void writeIntraframe(void) static void writeIntraframe(void)
{ {
blackboxValues_t *blackboxCurrent = blackboxHistory[0]; blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int x; int x;
blackboxWrite('I'); blackboxWrite('I');
@ -531,8 +573,8 @@ static void writeInterframe(void)
int x; int x;
int32_t deltas[8]; int32_t deltas[8];
blackboxValues_t *blackboxCurrent = blackboxHistory[0]; blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
blackboxValues_t *blackboxLast = blackboxHistory[1]; blackboxMainState_t *blackboxLast = blackboxHistory[1];
blackboxWrite('P'); blackboxWrite('P');
@ -638,6 +680,60 @@ static void writeInterframe(void)
blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing; blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
} }
/* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
* infrequently, delta updates are not reasonable, so we log independent frames. */
static void writeSlowFrame(void)
{
blackboxWrite('S');
blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
blackboxWriteUnsignedVB(slowHistory.stateFlags);
blackboxWriteUnsignedVB(slowHistory.failsafePhase);
blackboxSlowFrameIterationTimer = 0;
}
/**
* Load rarely-changing values from the FC into the given structure
*/
static void loadSlowState(blackboxSlowState_t *slow)
{
slow->flightModeFlags = flightModeFlags;
slow->stateFlags = stateFlags;
slow->failsafePhase = failsafePhase();
}
/**
* If the data in the slow frame has changed, log a slow frame.
*
* If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
* since the field was last logged.
*/
static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
{
// Write the slow frame peridocially so it can be recovered if we ever lose sync
bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
if (shouldWrite) {
loadSlowState(&slowHistory);
} else {
blackboxSlowState_t newSlowState;
loadSlowState(&newSlowState);
// Only write a slow frame if it was different from the previous state
if (memcmp(&newSlowState, &slowHistory, sizeof(slowHistory)) != 0) {
// Use the new state as our new history
memcpy(&slowHistory, &newSlowState, sizeof(slowHistory));
shouldWrite = true;
}
}
if (shouldWrite) {
writeSlowFrame();
}
}
static int gcd(int num, int denom) static int gcd(int num, int denom)
{ {
if (denom == 0) { if (denom == 0) {
@ -774,9 +870,9 @@ static void writeGPSFrame()
/** /**
* Fill the current state of the blackbox using values read from the flight controller * Fill the current state of the blackbox using values read from the flight controller
*/ */
static void loadBlackboxState(void) static void loadMainState(void)
{ {
blackboxValues_t *blackboxCurrent = blackboxHistory[0]; blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int i; int i;
blackboxCurrent->time = currentTime; blackboxCurrent->time = currentTime;
@ -839,6 +935,9 @@ static void loadBlackboxState(void)
* H Field I name:a,b,c * H Field I name:a,b,c
* H Field I predictor:0,1,2 * H Field I predictor:0,1,2
* *
* For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
* header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
*
* Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
* should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition. * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
* *
@ -849,15 +948,22 @@ static void loadBlackboxState(void)
* *
* Returns true if there is still header left to transmit (so call again to continue transmission). * Returns true if there is still header left to transmit (so call again to continue transmission).
*/ */
static bool sendFieldDefinition(const char * const *headerNames, unsigned int headerCount, const void *fieldDefinitions, static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const void *fieldDefinitions,
const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition) const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
{ {
const blackboxFieldDefinition_t *def; const blackboxFieldDefinition_t *def;
int charsWritten; int charsWritten;
unsigned int headerCount;
static bool needComma = false; static bool needComma = false;
size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions; size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
size_t conditionsStride = (char*) secondCondition - (char*) conditions; size_t conditionsStride = (char*) secondCondition - (char*) conditions;
if (deltaFrameChar) {
headerCount = BLACKBOX_DELTA_FIELD_HEADER_COUNT;
} else {
headerCount = BLACKBOX_SIMPLE_FIELD_HEADER_COUNT;
}
/* /*
* We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
* the whole header. * the whole header.
@ -867,10 +973,7 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
return false; //Someone probably called us again after we had already completed transmission return false; //Someone probably called us again after we had already completed transmission
} }
charsWritten = blackboxPrint("H Field "); charsWritten = blackboxPrintf("H Field %c %s:", xmitState.headerIndex >= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT ? deltaFrameChar : mainFrameChar, blackboxFieldHeaderNames[xmitState.headerIndex]);
charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]);
blackboxWrite(':');
charsWritten++;
xmitState.u.fieldIndex++; xmitState.u.fieldIndex++;
needComma = false; needComma = false;
@ -1051,83 +1154,42 @@ static void blackboxCheckAndLogArmingBeep()
} }
} }
/** /*
* Call each flight loop iteration to perform blackbox logging. * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
* the portion of logged loop iterations.
*/ */
void handleBlackbox(void) static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
{ {
int i; /* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
* recorded / skipped frames when the I frame's position is considered:
switch (blackboxState) {
case BLACKBOX_STATE_SEND_HEADER:
//On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
/*
* Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
* buffer.
*/ */
if (millis() > xmitState.u.startTime + 100) { return (pFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num;
for (i = 0; i < blackboxWriteChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) { }
blackboxWrite(blackboxHeader[xmitState.headerIndex]);
}
if (blackboxHeader[xmitState.headerIndex] == '\0') {
blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO);
}
}
break;
case BLACKBOX_STATE_SEND_FIELDINFO:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1,
ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
#ifdef GPS
if (feature(FEATURE_GPS)) {
blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS);
} else
#endif
blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
}
break;
#ifdef GPS
case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1,
ARRAY_LENGTH(blackboxGpsHFields), &blackboxGpsHFields[0].condition, &blackboxGpsHFields[1].condition)) {
blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS);
}
break;
case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1,
ARRAY_LENGTH(blackboxGpsGFields), &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
}
break;
#endif
case BLACKBOX_STATE_SEND_SYSINFO:
//On entry of this state, xmitState.headerIndex is 0
//Keep writing chunks of the system info headers until it returns true to signal completion
if (blackboxWriteSysinfo()) {
blackboxSetState(BLACKBOX_STATE_RUNNING);
}
break;
case BLACKBOX_STATE_RUNNING:
// On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
// Called once every FC loop in order to log the current state
static void blackboxLogIteration()
{
// Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
if (blackboxPFrameIndex == 0) { if (blackboxPFrameIndex == 0) {
// Copy current system values into the blackbox /*
loadBlackboxState(); * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
* an additional item to write at the same time)
*/
writeSlowFrameIfNeeded(false);
loadMainState();
writeIntraframe(); writeIntraframe();
} else { } else {
blackboxCheckAndLogArmingBeep(); blackboxCheckAndLogArmingBeep();
/* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
* recorded / skipped frames when the I frame's position is considered: /*
* We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
* So only log slow frames during loop iterations where we log a main frame.
*/ */
if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) { writeSlowFrameIfNeeded(true);
loadBlackboxState();
loadMainState();
writeInterframe(); writeInterframe();
} }
#ifdef GPS #ifdef GPS
@ -1153,6 +1215,10 @@ void handleBlackbox(void)
#endif #endif
} }
//Flush every iteration so that our runtime variance is minimized
blackboxDeviceFlush();
blackboxSlowFrameIterationTimer++;
blackboxIteration++; blackboxIteration++;
blackboxPFrameIndex++; blackboxPFrameIndex++;
@ -1160,6 +1226,80 @@ void handleBlackbox(void)
blackboxPFrameIndex = 0; blackboxPFrameIndex = 0;
blackboxIFrameIndex++; blackboxIFrameIndex++;
} }
}
/**
* Call each flight loop iteration to perform blackbox logging.
*/
void handleBlackbox(void)
{
int i;
switch (blackboxState) {
case BLACKBOX_STATE_SEND_HEADER:
//On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
/*
* Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
* buffer.
*/
if (millis() > xmitState.u.startTime + 100) {
for (i = 0; i < blackboxWriteChunkSize && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
blackboxWrite(blackboxHeader[xmitState.headerIndex]);
}
if (blackboxHeader[xmitState.headerIndex] == '\0') {
blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
}
}
break;
case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields),
&blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
#ifdef GPS
if (feature(FEATURE_GPS)) {
blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER);
} else
#endif
blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
}
break;
#ifdef GPS
case BLACKBOX_STATE_SEND_GPS_H_HEADER:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAY_LENGTH(blackboxGpsHFields),
NULL, NULL)) {
blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER);
}
break;
case BLACKBOX_STATE_SEND_GPS_G_HEADER:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAY_LENGTH(blackboxGpsGFields),
&blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
}
break;
#endif
case BLACKBOX_STATE_SEND_SLOW_HEADER:
//On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAY_LENGTH(blackboxSlowFields),
NULL, NULL)) {
blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
}
break;
case BLACKBOX_STATE_SEND_SYSINFO:
//On entry of this state, xmitState.headerIndex is 0
//Keep writing chunks of the system info headers until it returns true to signal completion
if (blackboxWriteSysinfo()) {
blackboxSetState(BLACKBOX_STATE_RUNNING);
}
break;
case BLACKBOX_STATE_RUNNING:
// On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
blackboxLogIteration();
break; break;
case BLACKBOX_STATE_SHUTTING_DOWN: case BLACKBOX_STATE_SHUTTING_DOWN:
//On entry of this state, startTime is set and a flush is performed //On entry of this state, startTime is set and a flush is performed

30
src/main/blackbox/blackbox.h
View file

@ -17,38 +17,8 @@
#pragma once #pragma once
#include <stdint.h>
#include "common/axis.h"
#include "flight/mixer.h"
#include "blackbox/blackbox_fielddefs.h" #include "blackbox/blackbox_fielddefs.h"
typedef struct blackboxValues_t {
uint32_t time;
int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT];
int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t accSmooth[XYZ_AXIS_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS];
uint16_t vbatLatest;
uint16_t amperageLatest;
#ifdef BARO
int32_t BaroAlt;
#endif
#ifdef MAG
int16_t magADC[XYZ_AXIS_COUNT];
#endif
#ifdef SONAR
int32_t sonarRaw;
#endif
uint16_t rssi;
} blackboxValues_t;
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data); void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void); void initBlackbox(void);

2
src/main/blackbox/blackbox_fielddefs.h
View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#include <stdint.h>
typedef enum FlightLogFieldCondition { typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0, FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0,
FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1,

4
src/main/common/printf.h
View file

@ -105,6 +105,10 @@ regs Kusti, 23.10.2004
#ifndef __TFP_PRINTF__ #ifndef __TFP_PRINTF__
#define __TFP_PRINTF__ #define __TFP_PRINTF__
#include <stdarg.h>
#include "drivers/serial.h"
void init_printf(void *putp, void (*putf) (void *, char)); void init_printf(void *putp, void (*putf) (void *, char));
int tfp_printf(const char *fmt, ...); int tfp_printf(const char *fmt, ...);

3
src/main/config/config.c
View file

@ -171,6 +171,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->dterm_cut_hz = 0;
pidProfile->pterm_cut_hz = 0;
pidProfile->gyro_cut_hz = 0;
pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
pidProfile->I_f[ROLL] = 0.6f; pidProfile->I_f[ROLL] = 0.6f;

8
src/main/drivers/accgyro_mpu6050.c
View file

@ -372,7 +372,9 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
if (lpf >= 188) if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ; mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98) else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ; mpuLowPassFilter = INV_FILTER_98HZ;
@ -382,8 +384,10 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_
mpuLowPassFilter = INV_FILTER_20HZ; mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10) else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ; mpuLowPassFilter = INV_FILTER_10HZ;
else else if (lpf > 0)
mpuLowPassFilter = INV_FILTER_5HZ; mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return true; return true;
} }

29
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -275,33 +275,22 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
int16_t data[3]; int16_t data[3];
// default lpf is 42Hz // default lpf is 42Hz
switch (lpf) { if (lpf == 256)
case 256:
mpuLowPassFilter = BITS_DLPF_CFG_256HZ; mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
break; else if (lpf >= 188)
case 188:
mpuLowPassFilter = BITS_DLPF_CFG_188HZ; mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
break; else if (lpf >= 98)
case 98:
mpuLowPassFilter = BITS_DLPF_CFG_98HZ; mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
break; else if (lpf >= 42)
default:
case 42:
mpuLowPassFilter = BITS_DLPF_CFG_42HZ; mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
break; else if (lpf >= 20)
case 20:
mpuLowPassFilter = BITS_DLPF_CFG_20HZ; mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
break; else if (lpf >= 10)
case 10:
mpuLowPassFilter = BITS_DLPF_CFG_10HZ; mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
break; else if (lpf > 0)
case 5:
mpuLowPassFilter = BITS_DLPF_CFG_5HZ; mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
break; else
case 0: mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF;
break;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

4
src/main/drivers/flash.h
View file

@ -20,10 +20,10 @@
#include <stdint.h> #include <stdint.h>
typedef struct flashGeometry_t { typedef struct flashGeometry_t {
uint8_t sectors; // Count of the number of erasable blocks on the device uint16_t sectors; // Count of the number of erasable blocks on the device
uint16_t pagesPerSector; uint16_t pagesPerSector;
uint16_t pageSize; // In bytes const uint16_t pageSize; // In bytes
uint32_t sectorSize; // This is just pagesPerSector * pageSize uint32_t sectorSize; // This is just pagesPerSector * pageSize

14
src/main/drivers/flash_m25p16.c
View file

@ -42,6 +42,8 @@
#define JEDEC_ID_MICRON_M25P16 0x202015 #define JEDEC_ID_MICRON_M25P16 0x202015
#define JEDEC_ID_MICRON_N25Q064 0x20BA17 #define JEDEC_ID_MICRON_N25Q064 0x20BA17
#define JEDEC_ID_WINBOND_W25Q64 0xEF4017 #define JEDEC_ID_WINBOND_W25Q64 0xEF4017
#define JEDEC_ID_MICRON_N25Q128 0x20ba18
#define JEDEC_ID_WINBOND_W25Q128 0xEF4018
#define DISABLE_M25P16 GPIO_SetBits(M25P16_CS_GPIO, M25P16_CS_PIN) #define DISABLE_M25P16 GPIO_SetBits(M25P16_CS_GPIO, M25P16_CS_PIN)
#define ENABLE_M25P16 GPIO_ResetBits(M25P16_CS_GPIO, M25P16_CS_PIN) #define ENABLE_M25P16 GPIO_ResetBits(M25P16_CS_GPIO, M25P16_CS_PIN)
@ -53,7 +55,7 @@
#define SECTOR_ERASE_TIMEOUT_MILLIS 5000 #define SECTOR_ERASE_TIMEOUT_MILLIS 5000
#define BULK_ERASE_TIMEOUT_MILLIS 21000 #define BULK_ERASE_TIMEOUT_MILLIS 21000
static flashGeometry_t geometry; static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE};
/* /*
* Whether we've performed an action that could have made the device busy for writes. * Whether we've performed an action that could have made the device busy for writes.
@ -148,23 +150,27 @@ static bool m25p16_readIdentification()
// Manufacturer, memory type, and capacity // Manufacturer, memory type, and capacity
chipID = (in[1] << 16) | (in[2] << 8) | (in[3]); chipID = (in[1] << 16) | (in[2] << 8) | (in[3]);
// All supported chips use the same pagesize of 256 bytes
switch (chipID) { switch (chipID) {
case JEDEC_ID_MICRON_M25P16: case JEDEC_ID_MICRON_M25P16:
geometry.sectors = 32; geometry.sectors = 32;
geometry.pagesPerSector = 256; geometry.pagesPerSector = 256;
geometry.pageSize = 256;
break; break;
case JEDEC_ID_MICRON_N25Q064: case JEDEC_ID_MICRON_N25Q064:
case JEDEC_ID_WINBOND_W25Q64: case JEDEC_ID_WINBOND_W25Q64:
geometry.sectors = 128; geometry.sectors = 128;
geometry.pagesPerSector = 256; geometry.pagesPerSector = 256;
geometry.pageSize = 256; break;
case JEDEC_ID_MICRON_N25Q128:
case JEDEC_ID_WINBOND_W25Q128:
geometry.sectors = 256;
geometry.pagesPerSector = 256;
break; break;
default: default:
// Unsupported chip or not an SPI NOR flash // Unsupported chip or not an SPI NOR flash
geometry.sectors = 0; geometry.sectors = 0;
geometry.pagesPerSector = 0; geometry.pagesPerSector = 0;
geometry.pageSize = 0;
geometry.sectorSize = 0; geometry.sectorSize = 0;
geometry.totalSize = 0; geometry.totalSize = 0;

2
src/main/drivers/flash_m25p16.h
View file

@ -20,6 +20,8 @@
#include <stdint.h> #include <stdint.h>
#include "flash.h" #include "flash.h"
#define M25P16_PAGESIZE 256
bool m25p16_init(); bool m25p16_init();
void m25p16_eraseSector(uint32_t address); void m25p16_eraseSector(uint32_t address);

27
src/main/flight/filter.c Normal file
View file

@ -0,0 +1,27 @@
/*
* filter.c
*
* Created on: 24 jun. 2015
* Author: borisb
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "common/axis.h"
#include "flight/filter.h"
extern uint16_t cycleTime;
// PT1 Low Pass filter
float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut) {
float dT = (float)cycleTime * 0.000001f;
float RC= 1.0f / ( 2.0f * (float)M_PI * f_cut );
*state = *state + dT / (RC + dT) * (input - *state);
return *state;
}

11
src/main/flight/filter.h Normal file
View file

@ -0,0 +1,11 @@
/*
* filter.h
*
* Created on: 24 jun. 2015
* Author: borisb
*/
typedef float filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut);

74
src/main/flight/pid.c
View file

@ -42,6 +42,7 @@
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/autotune.h" #include "flight/autotune.h"
#include "flight/filter.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
@ -93,6 +94,8 @@ void pidResetErrorGyro(void)
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t PTermState[3], DTermState[3];
#ifdef AUTOTUNE #ifdef AUTOTUNE
bool shouldAutotune(void) bool shouldAutotune(void)
{ {
@ -184,6 +187,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate P component // -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
// -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10 // -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f); errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f);
@ -202,6 +210,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output // -----calculate total PID output
@ -282,11 +296,23 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} }
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32; DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm; axisPID[axis] = PTerm + ITerm - DTerm;
@ -361,12 +387,22 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
DTerm = delta1[axis] + delta2[axis] + delta; DTerm = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm; axisPID[axis] = PTerm + ITerm - DTerm;
@ -474,11 +510,22 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
} }
PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADC[axis] - lastGyro[axis]) / 4; delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * dynD8[axis]) / 32; DTerm = (deltaSum * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm; axisPID[axis] = PTerm + ITerm - DTerm;
@ -529,8 +576,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
uint8_t axis; uint8_t axis;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale; float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // maincuthz (default 0 (disabled), Range 1-50Hz)
MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
@ -585,6 +631,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
} }
PTerm -= gyroADCQuant * dynP8[axis] * 0.003f; PTerm -= gyroADCQuant * dynP8[axis] * 0.003f;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS; delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroADCQuant; lastGyro[axis] = gyroADCQuant;
@ -631,6 +683,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
} }
} }
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = PTerm + ITerm;
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result.
@ -720,6 +778,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate P component // -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
// Pterm low pass
if (pidProfile->pterm_cut_hz) {
PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz);
}
// -----calculate I component // -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced. // there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
@ -743,6 +807,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; delta1[axis] = delta;
// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz);
}
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output // -----calculate total PID output

5
src/main/flight/pid.h
View file

@ -19,7 +19,6 @@
#define GYRO_I_MAX 256 // Gyro I limiter #define GYRO_I_MAX 256 // Gyro I limiter
#define RCconstPI 0.159154943092f // 0.5f / M_PI; #define RCconstPI 0.159154943092f // 0.5f / M_PI;
#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
#define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw. #define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw.
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
@ -63,6 +62,9 @@ typedef struct pidProfile_s {
float H_level; float H_level;
uint8_t H_sensitivity; uint8_t H_sensitivity;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300) uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames
uint8_t gyro_cut_hz; // Used for soft gyro filtering
} pidProfile_t; } pidProfile_t;
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
@ -75,4 +77,3 @@ void pidSetController(pidControllerType_e type);
void pidResetErrorAngle(void); void pidResetErrorAngle(void);
void pidResetErrorGyro(void); void pidResetErrorGyro(void);

29
src/main/io/flashfs.c
View file

@ -49,15 +49,10 @@ static uint8_t bufferHead = 0, bufferTail = 0;
// The position of the buffer's tail in the overall flash address space: // The position of the buffer's tail in the overall flash address space:
static uint32_t tailAddress = 0; static uint32_t tailAddress = 0;
// The index of the tail within the flash page it is inside
static uint16_t tailIndexInPage = 0;
static bool shouldFlush = false;
static void flashfsClearBuffer() static void flashfsClearBuffer()
{ {
bufferTail = bufferHead = 0; bufferTail = bufferHead = 0;
shouldFlush = false;
} }
static bool flashfsBufferIsEmpty() static bool flashfsBufferIsEmpty()
@ -68,10 +63,6 @@ static bool flashfsBufferIsEmpty()
static void flashfsSetTailAddress(uint32_t address) static void flashfsSetTailAddress(uint32_t address)
{ {
tailAddress = address; tailAddress = address;
if (m25p16_getGeometry()->pageSize > 0) {
tailIndexInPage = tailAddress % m25p16_getGeometry()->pageSize;
}
} }
void flashfsEraseCompletely() void flashfsEraseCompletely()
@ -157,8 +148,6 @@ static uint32_t flashfsTransmitBufferUsed()
*/ */
static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSizes, int bufferCount, bool sync) static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSizes, int bufferCount, bool sync)
{ {
const flashGeometry_t *geometry = m25p16_getGeometry();
uint32_t bytesTotal = 0; uint32_t bytesTotal = 0;
int i; int i;
@ -181,8 +170,8 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
* Each page needs to be saved in a separate program operation, so * Each page needs to be saved in a separate program operation, so
* if we would cross a page boundary, only write up to the boundary in this iteration: * if we would cross a page boundary, only write up to the boundary in this iteration:
*/ */
if (tailIndexInPage + bytesTotalRemaining > geometry->pageSize) { if (tailAddress % M25P16_PAGESIZE + bytesTotalRemaining > M25P16_PAGESIZE) {
bytesTotalThisIteration = geometry->pageSize - tailIndexInPage; bytesTotalThisIteration = M25P16_PAGESIZE - tailAddress % M25P16_PAGESIZE;
} else { } else {
bytesTotalThisIteration = bytesTotalRemaining; bytesTotalThisIteration = bytesTotalRemaining;
} }
@ -293,15 +282,14 @@ static void flashfsAdvanceTailInBuffer(uint32_t delta)
} }
/** /**
* If the flash is ready to accept writes, flush the buffer to it, otherwise schedule * If the flash is ready to accept writes, flush the buffer to it.
* a flush for later and return immediately.
* *
* Returns true if all data in the buffer has been flushed to the device. * Returns true if all data in the buffer has been flushed to the device, or false if
* there is still data to be written (call flush again later).
*/ */
bool flashfsFlushAsync() bool flashfsFlushAsync()
{ {
if (flashfsBufferIsEmpty()) { if (flashfsBufferIsEmpty()) {
shouldFlush = false;
return true; // Nothing to flush return true; // Nothing to flush
} }
@ -313,8 +301,6 @@ bool flashfsFlushAsync()
bytesWritten = flashfsWriteBuffers(buffers, bufferSizes, 2, false); bytesWritten = flashfsWriteBuffers(buffers, bufferSizes, 2, false);
flashfsAdvanceTailInBuffer(bytesWritten); flashfsAdvanceTailInBuffer(bytesWritten);
shouldFlush = !flashfsBufferIsEmpty();
return flashfsBufferIsEmpty(); return flashfsBufferIsEmpty();
} }
@ -327,7 +313,6 @@ bool flashfsFlushAsync()
void flashfsFlushSync() void flashfsFlushSync()
{ {
if (flashfsBufferIsEmpty()) { if (flashfsBufferIsEmpty()) {
shouldFlush = false;
return; // Nothing to flush return; // Nothing to flush
} }
@ -366,7 +351,7 @@ void flashfsWriteByte(uint8_t byte)
bufferHead = 0; bufferHead = 0;
} }
if (shouldFlush || flashfsTransmitBufferUsed() >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) { if (flashfsTransmitBufferUsed() >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) {
flashfsFlushAsync(); flashfsFlushAsync();
} }
} }
@ -393,7 +378,7 @@ void flashfsWrite(const uint8_t *data, unsigned int len, bool sync)
* Would writing this data to our buffer cause our buffer to reach the flush threshold? If so try to write through * Would writing this data to our buffer cause our buffer to reach the flush threshold? If so try to write through
* to the flash now * to the flash now
*/ */
if (shouldFlush || bufferSizes[0] + bufferSizes[1] + bufferSizes[2] >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) { if (bufferSizes[0] + bufferSizes[1] + bufferSizes[2] >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) {
uint32_t bytesWritten; uint32_t bytesWritten;
// Attempt to write all three buffers through to the flash asynchronously // Attempt to write all three buffers through to the flash asynchronously

3
src/main/io/serial_cli.c
View file

@ -486,6 +486,9 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
{ "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
#ifdef BLACKBOX #ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },

11
src/main/mw.c
View file

@ -72,6 +72,7 @@
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/autotune.h" #include "flight/autotune.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/filter.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
@ -740,6 +741,16 @@ void loop(void)
cycleTime = (int32_t)(currentTime - previousTime); cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime; previousTime = currentTime;
// Gyro Low Pass
if (currentProfile->pidProfile.gyro_cut_hz) {
int axis;
static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz);
}
}
annexCode(); annexCode();
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true; haveProcessedAnnexCodeOnce = true;

115
src/main/startup/startup_stm32f10x_hd_gcc.S
View file

@ -4,7 +4,7 @@
* @author MCD Application Team * @author MCD Application Team
* @version V3.5.0 * @version V3.5.0
* @date 11-March-2011 * @date 11-March-2011
* @brief STM32F10x Medium Density Devices vector table for Atollic toolchain. * @brief STM32F10x High Density Devices vector table for Atollic toolchain.
* This module performs: * This module performs:
* - Set the initial SP * - Set the initial SP
* - Set the initial PC == Reset_Handler, * - Set the initial PC == Reset_Handler,
@ -47,8 +47,9 @@ defined in linker script */
.word _sbss .word _sbss
/* end address for the .bss section. defined in linker script */ /* end address for the .bss section. defined in linker script */
.word _ebss .word _ebss
/* stack used for SystemInit_ExtMemCtl; always internal RAM used */
.equ BootRAM, 0xF108F85F .equ BootRAM, 0xF1E0F85F
/** /**
* @brief This is the code that gets called when the processor first * @brief This is the code that gets called when the processor first
* starts execution following a reset event. Only the absolutely * starts execution following a reset event. Only the absolutely
@ -230,6 +231,23 @@ g_pfnVectors:
.word EXTI15_10_IRQHandler .word EXTI15_10_IRQHandler
.word RTCAlarm_IRQHandler .word RTCAlarm_IRQHandler
.word USBWakeUp_IRQHandler .word USBWakeUp_IRQHandler
.word TIM8_BRK_IRQHandler
.word TIM8_UP_IRQHandler
.word TIM8_TRG_COM_IRQHandler
.word TIM8_CC_IRQHandler
.word ADC3_IRQHandler
.word FSMC_IRQHandler
.word SDIO_IRQHandler
.word TIM5_IRQHandler
.word SPI3_IRQHandler
.word UART4_IRQHandler
.word UART5_IRQHandler
.word TIM6_IRQHandler
.word TIM7_IRQHandler
.word DMA2_Channel1_IRQHandler
.word DMA2_Channel2_IRQHandler
.word DMA2_Channel3_IRQHandler
.word DMA2_Channel4_5_IRQHandler
.word 0 .word 0
.word 0 .word 0
.word 0 .word 0
@ -237,9 +255,45 @@ g_pfnVectors:
.word 0 .word 0
.word 0 .word 0
.word 0 .word 0
.word BootRAM /* @0x108. This is for boot in RAM mode for .word 0
STM32F10x Medium Density devices. */ .word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word BootRAM /* @0x1E0. This is for boot in RAM mode for
STM32F10x High Density devices. */
/******************************************************************************* /*******************************************************************************
* *
* Provide weak aliases for each Exception handler to the Default_Handler. * Provide weak aliases for each Exception handler to the Default_Handler.
@ -404,5 +458,56 @@ g_pfnVectors:
.weak USBWakeUp_IRQHandler .weak USBWakeUp_IRQHandler
.thumb_set USBWakeUp_IRQHandler,Default_Handler .thumb_set USBWakeUp_IRQHandler,Default_Handler
.weak TIM8_BRK_IRQHandler
.thumb_set TIM8_BRK_IRQHandler,Default_Handler
.weak TIM8_UP_IRQHandler
.thumb_set TIM8_UP_IRQHandler,Default_Handler
.weak TIM8_TRG_COM_IRQHandler
.thumb_set TIM8_TRG_COM_IRQHandler,Default_Handler
.weak TIM8_CC_IRQHandler
.thumb_set TIM8_CC_IRQHandler,Default_Handler
.weak ADC3_IRQHandler
.thumb_set ADC3_IRQHandler,Default_Handler
.weak FSMC_IRQHandler
.thumb_set FSMC_IRQHandler,Default_Handler
.weak SDIO_IRQHandler
.thumb_set SDIO_IRQHandler,Default_Handler
.weak TIM5_IRQHandler
.thumb_set TIM5_IRQHandler,Default_Handler
.weak SPI3_IRQHandler
.thumb_set SPI3_IRQHandler,Default_Handler
.weak UART4_IRQHandler
.thumb_set UART4_IRQHandler,Default_Handler
.weak UART5_IRQHandler
.thumb_set UART5_IRQHandler,Default_Handler
.weak TIM6_IRQHandler
.thumb_set TIM6_IRQHandler,Default_Handler
.weak TIM7_IRQHandler
.thumb_set TIM7_IRQHandler,Default_Handler
.weak DMA2_Channel1_IRQHandler
.thumb_set DMA2_Channel1_IRQHandler,Default_Handler
.weak DMA2_Channel2_IRQHandler
.thumb_set DMA2_Channel2_IRQHandler,Default_Handler
.weak DMA2_Channel3_IRQHandler
.thumb_set DMA2_Channel3_IRQHandler,Default_Handler
.weak DMA2_Channel4_5_IRQHandler
.thumb_set DMA2_Channel4_5_IRQHandler,Default_Handler
/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/