1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00

Merge branch 'master' into bugfix-ground

This commit is contained in:
elafargue 2011-05-09 16:50:11 +02:00
commit 1c09f85be2
23 changed files with 1184 additions and 289 deletions

View File

@ -14,7 +14,7 @@
height="80.827866"
id="svg10068"
version="1.1"
inkscape:version="0.47 r22583"
inkscape:version="0.48.0 r9654"
sodipodi:docname="arm-status.svg"
inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png"
inkscape:export-xdpi="103.61"
@ -828,6 +828,402 @@
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 24 : 1"
sodipodi:type="inkscape:persp3d" />
<linearGradient
id="linearGradient5128-3">
<stop
offset="0"
style="stop-color:#e5e5e5;stop-opacity:1"
id="stop5130-1" />
<stop
offset="1"
style="stop-color:#ababab;stop-opacity:1"
id="stop5132-1" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.09614231,0,0,0.09614231,1.8468935,1.9430362)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-3"
id="linearGradient3260"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient3397-90">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-8" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-1" />
</linearGradient>
<linearGradient
gradientTransform="translate(-17.058189,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-90"
id="linearGradient3264"
y2="16.004715"
x2="21"
y1="0"
x1="21" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-6"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-2" />
<stop
offset="1"
style="stop-color:#cccccc;stop-opacity:1"
id="stop3313-9" />
</linearGradient>
<linearGradient
gradientTransform="translate(-17.058189,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-6"
id="linearGradient3262"
y2="16.000105"
x2="25"
y1="0"
x1="25" />
<linearGradient
id="linearGradient5128-6">
<stop
offset="0"
style="stop-color:#eeeeee;stop-opacity:1"
id="stop5130-43" />
<stop
offset="1"
style="stop-color:#a2a2a2;stop-opacity:1"
id="stop5132-0" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.1562313,0,0,0.1562313,2.0012016,2.1574324)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-6"
id="linearGradient3241"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient3397-9">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-7" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-7" />
</linearGradient>
<linearGradient
gradientTransform="translate(-24.5,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-9"
id="linearGradient3228"
y2="23.019524"
x2="40"
y1="1"
x1="40" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-4"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-8" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313-88" />
</linearGradient>
<linearGradient
gradientTransform="translate(-24.5,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-4"
id="linearGradient3218"
y2="23.00024"
x2="34"
y1="1"
x1="34" />
<linearGradient
id="linearGradient10691-8">
<stop
offset="0"
style="stop-color:#000000;stop-opacity:1"
id="stop10693-6" />
<stop
offset="1"
style="stop-color:#000000;stop-opacity:0"
id="stop10695-0" />
</linearGradient>
<radialGradient
gradientTransform="matrix(1.660115,0,0,0.3458573,0.8727276,-3.9605294)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient10691-8"
id="radialGradient2411"
fy="73.615715"
fx="6.702713"
r="7.228416"
cy="73.615715"
cx="6.702713" />
<linearGradient
gradientTransform="matrix(0.2014014,0,0,0.2014015,3.1103045,3.311705)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-0"
id="linearGradient2868"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient5128-0">
<stop
offset="0"
style="stop-color:#ffffff;stop-opacity:1"
id="stop5130-4" />
<stop
offset="1"
style="stop-color:#959595;stop-opacity:1"
id="stop5132-4" />
</linearGradient>
<linearGradient
id="linearGradient3397-1">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-0" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-3" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-1"
id="linearGradient3328"
y2="47.044857"
x2="25.922546"
y1="19"
x1="25.922546" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-8"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-4" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313-8" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-8"
id="linearGradient3326"
y2="47.012184"
x2="32.036148"
y1="19"
x1="32.036148" />
<linearGradient
id="linearGradient10691">
<stop
offset="0"
style="stop-color:#000000;stop-opacity:1"
id="stop10693" />
<stop
offset="1"
style="stop-color:#000000;stop-opacity:0"
id="stop10695" />
</linearGradient>
<radialGradient
gradientTransform="matrix(2.2134617,0,0,0.4842001,1.1638028,-7.1447362)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient10691"
id="radialGradient2538"
fy="73.615715"
fx="6.702713"
r="7.228416"
cy="73.615715"
cx="6.702713" />
<linearGradient
gradientTransform="matrix(0.3244803,0,0,0.3244803,3.2332655,2.5577454)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128"
id="linearGradient2512"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient5128">
<stop
offset="0"
style="stop-color:#e5e5e5;stop-opacity:1"
id="stop5130" />
<stop
offset="1"
style="stop-color:#ababab;stop-opacity:1"
id="stop5132" />
</linearGradient>
<linearGradient
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309"
id="linearGradient2515"
y2="45.017357"
x2="20.758585"
y1="1"
x1="20.758585" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313" />
</linearGradient>
<linearGradient
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397"
id="linearGradient2517"
y2="45"
x2="37.201294"
y1="1"
x1="37.201294" />
<linearGradient
id="linearGradient3397">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401" />
</linearGradient>
<radialGradient
gradientTransform="matrix(2.1647059,0,0,0.7529402,-111.56471,36.517647)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient8838-9"
id="radialGradient2436-5"
fy="4.625"
fx="62.625"
r="10.625"
cy="4.625"
cx="62.625" />
<linearGradient
id="linearGradient8838-9">
<stop
offset="0"
style="stop-color:#000000;stop-opacity:1"
id="stop8840-7" />
<stop
offset="1"
style="stop-color:#000000;stop-opacity:0"
id="stop8842-8" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-8-1"
id="linearGradient3326-4"
y2="47.012184"
x2="32.036148"
y1="19"
x1="32.036148" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-8-1"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-4-5" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313-8-5" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-1-9"
id="linearGradient3328-4"
y2="47.044857"
x2="25.922546"
y1="19"
x1="25.922546" />
<linearGradient
id="linearGradient3397-1-9">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-0-8" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-3-3" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.2014014,0,0,0.2014015,3.1103045,3.311705)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-0-5"
id="linearGradient2868-8"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient5128-0-5">
<stop
offset="0"
style="stop-color:#ffffff;stop-opacity:1"
id="stop5130-4-2" />
<stop
offset="1"
style="stop-color:#959595;stop-opacity:1"
id="stop5132-4-2" />
</linearGradient>
<linearGradient
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919"
gradientTransform="matrix(0.2014014,0,0,0.2014015,3.1103045,3.311705)"
gradientUnits="userSpaceOnUse"
id="linearGradient3339"
xlink:href="#linearGradient5128-0-5"
inkscape:collect="always" />
</defs>
<sodipodi:namedview
id="base"
@ -836,14 +1232,14 @@
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="1.4"
inkscape:cx="44.030427"
inkscape:cy="102.26308"
inkscape:zoom="1.979899"
inkscape:cx="69.908261"
inkscape:cy="99.737699"
inkscape:document-units="px"
inkscape:current-layer="layer1"
inkscape:current-layer="background"
showgrid="false"
inkscape:window-width="1366"
inkscape:window-height="693"
inkscape:window-height="691"
inkscape:window-x="0"
inkscape:window-y="24"
inkscape:window-maximized="1"
@ -861,7 +1257,7 @@
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
<dc:title />
<dc:creator>
<cc:Agent>
<dc:title>Edouard Lafargue</dc:title>
@ -977,7 +1373,7 @@
d="m 53.950159,30.352861 c 0,13.351915 -10.823865,24.17578 -24.175779,24.17578 -13.351915,0 -24.1757796,-10.823865 -24.1757796,-24.17578 0,-13.351914 10.8238646,-24.1757789 24.1757796,-24.1757789 13.351914,0 24.175779,10.8238649 24.175779,24.1757789 z" />
</g>
<g
id="symbol-False"
id="symbol-Disarmed"
transform="translate(173.85714,-69.957837)"
inkscape:label="#layer1-6">
<path
@ -998,7 +1394,7 @@
d="M 17.049914,14.239243 33.752151,33.92599" />
</g>
<g
id="symbol-True"
id="symbol-Armed"
transform="translate(92.42857,-72.302217)"
inkscape:label="#layer1-0">
<path
@ -1015,5 +1411,26 @@
id="path1544"
d="M 32.938536,7.2012775 26.730268,18.60791 21.197213,28.668822 c -0.178482,0.181096 -0.123845,0.754094 -0.902701,0.952966 -0.538869,0.137595 -0.636352,-0.06289 -1.204172,-0.427914 l -6.950556,-5.492153 c -0.7733,-0.621665 -0.749753,-0.589365 -1.297288,-0.03737 L 8.0515824,27.18582 c -0.7518117,0.804672 -0.6353589,1.290849 0.3017539,1.891047 0,0 13.0781537,10.780055 13.0949407,10.788373 0.155564,0.085 0.0892,0.114145 0.757862,0.57814 0.428927,0.297637 0.807539,-0.647982 1.083435,-1.15286 L 39.190779,10.274963 C 39.55113,9.6155282 39.523767,9.624421 38.860008,9.2617466 L 34.11801,6.7472827 C 33.434075,6.3758505 33.430508,6.3800895 32.938536,7.2012775 z" />
</g>
<g
style="display:inline"
id="symbol-Arming"
transform="matrix(1.2857213,0,0,1.2857213,18.29593,-68.120293)"
inkscape:label="#layer1-33">
<path
style="fill:url(#linearGradient3326-4);fill-opacity:1;fill-rule:nonzero;stroke:url(#linearGradient3328-4);stroke-width:0.77777356;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0;marker:none;visibility:visible;display:block;overflow:visible;enable-background:accumulate"
id="rect2576"
d="m 18.636572,2.5905216 c -0.703275,-0.2569307 -1.495689,0.08705 -1.752619,0.7903248 l -0.600515,1.6437399 c -0.499176,-0.013219 -0.993955,0.00248 -1.484533,0.056512 L 14.072373,3.5181351 C 13.756758,2.8391668 12.960838,2.5483897 12.28187,2.864003 L 8.3651471,4.6602954 C 7.6861782,4.9759083 7.3954033,5.7718319 7.7110151,6.4507982 l 0.726532,1.5629636 C 8.0724464,8.359396 7.744193,8.7441155 7.4266456,9.141602 L 5.8122578,8.5518108 C 5.1089821,8.2948801 4.3165666,8.6388602 4.0596381,9.3421356 L 2.5797984,13.392781 c -0.2569313,0.703275 0.1271272,1.477063 0.8304007,1.733991 l 1.6143878,0.589791 c -0.013469,0.508577 -0.010523,1.014295 0.045789,1.513885 l -1.5629633,0.726533 c -0.6789688,0.315613 -0.9697472,1.111534 -0.654132,1.790502 l 1.8363684,3.898094 c 0.3156123,0.678968 1.1115366,0.969744 1.7905029,0.654132 l 1.5629636,-0.726532 c 0.3402133,0.357549 0.7083491,0.688501 1.0984877,1.000178 l -0.6005147,1.64374 c -0.2569313,0.703275 0.1271291,1.477064 0.8304008,1.733991 l 4.0212927,1.469116 c 0.703276,0.256931 1.466338,-0.09777 1.723267,-0.801048 l 0.600515,-1.64374 c 0.508577,0.01347 1.014295,0.01052 1.513885,-0.04579 l 0.726532,1.562963 c 0.315614,0.678968 1.111534,0.969745 1.790502,0.654132 l 3.916723,-1.796292 c 0.678969,-0.315613 0.969742,-1.111538 0.654132,-1.790503 l -0.745161,-1.603039 c 0.357549,-0.340214 0.6885,-0.708349 1.000178,-1.098488 l 1.64374,0.600515 c 0.703276,0.25693 1.466338,-0.09778 1.723267,-0.801049 l 1.47984,-4.050645 c 0.256931,-0.703275 -0.09778,-1.466339 -0.801048,-1.723268 l -1.64374,-0.600514 c 0.01322,-0.499176 -0.0025,-0.993955 -0.05651,-1.484533 l 1.60304,-0.745162 C 29.20091,13.73813 29.491686,12.942208 29.176073,12.26324 L 27.339704,8.365146 C 27.024092,7.6861784 26.228168,7.3954 25.549202,7.711014 L 23.986239,8.4375465 C 23.640605,8.0724458 23.255886,7.7441924 22.858399,7.4266449 L 23.458914,5.782905 C 23.715845,5.0796306 23.361136,4.3165668 22.657865,4.0596378 L 18.636572,2.5905216 z M 17.20103,12.71252 c 1.81469,0.662968 2.74942,2.67382 2.086452,4.48851 -0.662969,1.814689 -2.673821,2.749419 -4.48851,2.086451 -1.814691,-0.662969 -2.74942,-2.673821 -2.086452,-4.48851 0.662968,-1.81469 2.673821,-2.74942 4.48851,-2.086451 z"
inkscape:connector-curvature="0" />
<path
style="opacity:0.05;fill:#000000;fill-opacity:1;stroke:none"
id="path3315"
d="m 16.000001,7.9999996 c -4.411273,0 -7.9999985,3.5887274 -7.9999985,8.0000004 0,4.411274 3.5887255,8 7.9999985,8 C 20.411274,24 24,20.411274 24,16 24,11.588727 20.411274,7.9999996 16.000001,7.9999996 z m 0,3.6923094 c 2.377844,0 4.307693,1.929846 4.307693,4.307691 0,2.377847 -1.929849,4.307693 -4.307693,4.307693 -2.377846,0 -4.307691,-1.929846 -4.307691,-4.307693 0,-2.377845 1.929845,-4.307691 4.307691,-4.307691 z"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:url(#linearGradient3339);stroke-width:0.77777356;stroke-miterlimit:4;stroke-dasharray:none"
id="path28"
d="m 16.000001,7.6206906 c -4.620353,0 -8.3793085,3.7589574 -8.3793085,8.3793084 0,4.620352 3.7589555,8.37931 8.3793085,8.37931 4.620353,0 8.379309,-3.758958 8.379309,-8.37931 0,-4.620351 -3.758956,-8.3793084 -8.379309,-8.3793084 z"
inkscape:connector-curvature="0" />
</g>
</g>
</svg>

Before

Width:  |  Height:  |  Size: 37 KiB

After

Width:  |  Height:  |  Size: 52 KiB

View File

@ -151,6 +151,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flightstatus.c
SRC += $(OPUAVSYNTHDIR)/systemstats.c
SRC += $(OPUAVSYNTHDIR)/systemalarms.c
SRC += $(OPUAVSYNTHDIR)/systemsettings.c

View File

@ -96,6 +96,7 @@
#define PIOS_STABILIZATION_STACK_SIZE 624
#define PIOS_TELEM_STACK_SIZE 500
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
//#define PIOS_QUATERNION_STABILIZATION
#endif /* PIOS_CONFIG_H */

View File

@ -37,7 +37,7 @@
#include "systemsettings.h"
#include "actuatordesired.h"
#include "actuatorcommand.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "mixersettings.h"
#include "mixerstatus.h"
@ -135,7 +135,7 @@ static void actuatorTask(void* parameters)
MixerSettingsData mixerSettings;
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
ManualControlCommandData manualControl;
FlightStatusData flightStatus;
ActuatorSettingsGet(&settings);
PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
@ -165,7 +165,7 @@ static void actuatorTask(void* parameters)
lastSysTime = thisSysTime;
ManualControlCommandGet(&manualControl);
FlightStatusGet(&flightStatus);
SystemSettingsGet(&sysSettings);
MixerStatusGet(&mixerStatus);
MixerSettingsGet (&mixerSettings);
@ -190,10 +190,9 @@ static void actuatorTask(void* parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE;
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00;
bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE &&
manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE;
bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);

View File

@ -53,6 +53,7 @@
#include "attituderaw.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "flightstatus.h"
#include "CoordinateConversions.h"
#include "pios_flash_w25x.h"
@ -87,6 +88,7 @@ static int16_t accelbias[3];
static float q[4] = {1,0,0,0};
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
/**
* Initialise the module, called on startup
@ -133,15 +135,25 @@ static void AttitudeTask(void *parameters)
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
zero_during_arming = false;
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(xTaskGetTickCount() < 10000) {
// For first 5 seconds use accels to get gyro bias
accelKp = 1;
// Decrease the rate of gyro learning during init
accelKi = .5 / (1 + xTaskGetTickCount() / 5000);
yawBiasRate = 0.01 / (1 + xTaskGetTickCount() / 5000);
init = 0;
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKi = .01;
yawBiasRate = 0.1;
init = 0;
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
init = 1;
@ -313,6 +325,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];

View File

@ -35,12 +35,12 @@
typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3} flightmode_path;
#define PARSE_FLIGHT_MODE(x) ( \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
(x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
)
int32_t ManualControlInitialize();

View File

@ -41,6 +41,7 @@
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
@ -54,6 +55,7 @@
#define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150
@ -70,13 +72,16 @@ typedef enum
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
@ -125,12 +130,12 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) MANUALCONTROLCOMMAND_FLIGHTMODE_POSITIONHOLD) \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
@ -159,18 +164,16 @@ static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
portTickType lastSysTime;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
ManualControlCommandSet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Main task loop
@ -211,7 +214,7 @@ static void manualControlTask(void *parameters)
#elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
#endif
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0);
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Check settings, if error raise alarm
@ -227,38 +230,23 @@ static void manualControlTask(void *parameters)
}
// decide if we have valid manual input or not
bool valid_input_detected = TRUE;
if (!validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]))
valid_input_detected = FALSE;
if (!validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]))
valid_input_detected = FALSE;
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
// Implement hysteresis loop on connection status
if (valid_input_detected)
{
if (++connected_count > 10)
{
connection_state = CONNECTED;
connected_count = 0;
disconnected_count = 0;
}
}
else
{
if (++disconnected_count > 10)
{
connection_state = DISCONNECTED;
connected_count = 0;
disconnected_count = 0;
}
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (connection_state == DISCONNECTED) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
@ -269,161 +257,22 @@ static void manualControlTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaledChannel[settings.Accessory1];
else
cmd.Accessory1 = 0;
// Set accessory channels
cmd.Accessory1 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) ? scaledChannel[settings.Accessory1] : 0;
cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0;
cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaledChannel[settings.Accessory2];
else
cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaledChannel[settings.Accessory3];
else
cmd.Accessory3 = 0;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
cmd.FlightMode = settings.FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
cmd.FlightMode = settings.FlightModePosition[2];
else
cmd.FlightMode = settings.FlightModePosition[1];
//
// Arming and Disarming mechanism
//
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
} else {
// In all other cases, we will not change the arm state when disconnected
if (connection_state == CONNECTED)
{
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
if (cmd.Throttle < 0) {
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
}
} else {
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
if (cmd.Throttle < 0) {
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
if (connection_state == CONNECTED) {
// Should use RC input only if RX is connected
if (armingInputLevel <= -0.50)
manualArm = true;
else if (armingInputLevel >= +0.50)
manualDisarm = true;
}
// Swap arm-disarming see assumptions8
if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) {
bool temp = manualArm;
manualArm = manualDisarm;
manualDisarm = temp;
}
switch(armState) {
case ARM_STATE_DISARMED:
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
if (manualArm)
{
if (okToArm()) // only allow arming if it's OK too
{
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
}
break;
case ARM_STATE_ARMING_MANUAL:
if (manualArm) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_ARMED;
}
else
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if (settings.ArmedTimeout != 0)
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout)
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_DISARMED;
}
else
armState = ARM_STATE_ARMED;
break;
} // End Switch
} else {
// The throttle is not low, in case we where arming or disarming, abort
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
}
}
}
}
//
// End of arming/disarming
//
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
@ -435,8 +284,10 @@ static void manualControlTask(void *parameters)
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(cmd.FlightMode)) {
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@ -450,7 +301,7 @@ static void manualControlTask(void *parameters)
case FLIGHTMODE_GUIDANCE:
// TODO: Implement
break;
}
}
}
}
@ -474,14 +325,16 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
switch(cmd->FlightMode) {
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1:
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
@ -517,7 +370,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent)
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
@ -537,20 +390,6 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
valueScaled = 0;
}
// Neutral RC stick position dead band
if (deadband_percent > 0)
{
if (deadband_percent > 50) deadband_percent = 50; // limit deadband to a maximum of 50%
float deadband = (float)deadband_percent / 100;
if (fabs(valueScaled) <= deadband)
valueScaled = 0; // deadband the value
else
if (valueScaled < 0)
valueScaled = (valueScaled + deadband) / (1.0 - deadband); // value scales 0.0 to -1.0 after deadband
else
valueScaled = (valueScaled - deadband) / (1.0 - deadband); // value scales 0.0 to +1.0 after deadband
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0;
else
@ -591,6 +430,161 @@ static bool okToArm(void)
return true;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
}
/**
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
else
newMode = settings->FlightModePosition[1];
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
@ -606,16 +600,6 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
}
//
//static void armingMechanism(uint8_t* armingState, const ManualControlSettingsData* settings, const ManualControlCommandData* cmd)
//{
// if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// *armingState = MANUALCONTROLCOMMAND_ARMED_FALSE;
// return;
// }
//
//
//}
/**
* @}
* @}

View File

@ -39,9 +39,10 @@
#include "stabilizationdesired.h"
#include "attitudeactual.h"
#include "attituderaw.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "systemsettings.h"
#include "ahrssettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "CoordinateConversions.h"
// Private constants
@ -125,7 +126,7 @@ static void stabilizationTask(void* parameters)
AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
ManualControlCommandData manualControl;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
@ -148,13 +149,13 @@ static void stabilizationTask(void* parameters)
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
ManualControlCommandGet(&manualControl);
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw);
RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings);
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
@ -261,11 +262,8 @@ static void stabilizationTask(void* parameters)
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL)
{
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
shouldUpdate = 0;
}
if(shouldUpdate)
{
@ -275,14 +273,15 @@ static void stabilizationTask(void* parameters)
ActuatorDesiredSet(&actuatorDesired);
}
if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE ||
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
!shouldUpdate || (stabDesired.Throttle < 0))
{
ZeroPids();
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}

View File

@ -41,7 +41,7 @@
#include "openpilot.h"
#include "systemmod.h"
#include "objectpersistence.h"
#include "manualcontrolcommand.h"
#include "flightstatus.h"
#include "systemstats.h"
#include "i2cstats.h"
#include "watchdogstatus.h"
@ -52,9 +52,12 @@
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 1000
#define LED_BLINK_RATE_HZ 5
#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
#endif
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
@ -139,11 +142,11 @@ static void systemTask(void *parameters)
}
#endif
ManualControlCommandData manualControlCommandData;
ManualControlCommandGet(&manualControlCommandData);
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Wait until next period
if(manualControlCommandData.Armed == MANUALCONTROLCOMMAND_ARMED_TRUE) {
if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) );
} else {
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);

View File

@ -9,6 +9,7 @@
/* Begin PBXFileReference section */
65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = "<group>"; };
6502584212CA4D2600583CDF /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps13state.c; path = ../../AHRS/insgps13state.c; sourceTree = SOURCE_ROOT; };
65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = "<group>"; };
6509C7E912CA57DC002E5DC2 /* insgps16state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps16state.c; path = ../../AHRS/insgps16state.c; sourceTree = SOURCE_ROOT; };
650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = "<group>"; };
650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = "<group>"; };
@ -7383,6 +7384,7 @@
65C35E5E12EFB2F3004811C2 /* flightplansettings.xml */,
65C35E5F12EFB2F3004811C2 /* flightplanstatus.xml */,
65C35E6012EFB2F3004811C2 /* flighttelemetrystats.xml */,
65078B09136FCEE600536549 /* flightstatus.xml */,
65C35E6112EFB2F3004811C2 /* gcstelemetrystats.xml */,
65C35E6212EFB2F3004811C2 /* gpsposition.xml */,
65C35E6312EFB2F3004811C2 /* gpssatellites.xml */,

View File

@ -14,7 +14,7 @@
height="80.827866"
id="svg10068"
version="1.1"
inkscape:version="0.47 r22583"
inkscape:version="0.48.0 r9654"
sodipodi:docname="arm-status.svg"
inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png"
inkscape:export-xdpi="103.61"
@ -828,6 +828,402 @@
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 24 : 1"
sodipodi:type="inkscape:persp3d" />
<linearGradient
id="linearGradient5128-3">
<stop
offset="0"
style="stop-color:#e5e5e5;stop-opacity:1"
id="stop5130-1" />
<stop
offset="1"
style="stop-color:#ababab;stop-opacity:1"
id="stop5132-1" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.09614231,0,0,0.09614231,1.8468935,1.9430362)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-3"
id="linearGradient3260"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient3397-90">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-8" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-1" />
</linearGradient>
<linearGradient
gradientTransform="translate(-17.058189,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-90"
id="linearGradient3264"
y2="16.004715"
x2="21"
y1="0"
x1="21" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-6"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-2" />
<stop
offset="1"
style="stop-color:#cccccc;stop-opacity:1"
id="stop3313-9" />
</linearGradient>
<linearGradient
gradientTransform="translate(-17.058189,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-6"
id="linearGradient3262"
y2="16.000105"
x2="25"
y1="0"
x1="25" />
<linearGradient
id="linearGradient5128-6">
<stop
offset="0"
style="stop-color:#eeeeee;stop-opacity:1"
id="stop5130-43" />
<stop
offset="1"
style="stop-color:#a2a2a2;stop-opacity:1"
id="stop5132-0" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.1562313,0,0,0.1562313,2.0012016,2.1574324)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-6"
id="linearGradient3241"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient3397-9">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-7" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-7" />
</linearGradient>
<linearGradient
gradientTransform="translate(-24.5,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-9"
id="linearGradient3228"
y2="23.019524"
x2="40"
y1="1"
x1="40" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-4"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-8" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313-88" />
</linearGradient>
<linearGradient
gradientTransform="translate(-24.5,0)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-4"
id="linearGradient3218"
y2="23.00024"
x2="34"
y1="1"
x1="34" />
<linearGradient
id="linearGradient10691-8">
<stop
offset="0"
style="stop-color:#000000;stop-opacity:1"
id="stop10693-6" />
<stop
offset="1"
style="stop-color:#000000;stop-opacity:0"
id="stop10695-0" />
</linearGradient>
<radialGradient
gradientTransform="matrix(1.660115,0,0,0.3458573,0.8727276,-3.9605294)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient10691-8"
id="radialGradient2411"
fy="73.615715"
fx="6.702713"
r="7.228416"
cy="73.615715"
cx="6.702713" />
<linearGradient
gradientTransform="matrix(0.2014014,0,0,0.2014015,3.1103045,3.311705)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-0"
id="linearGradient2868"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient5128-0">
<stop
offset="0"
style="stop-color:#ffffff;stop-opacity:1"
id="stop5130-4" />
<stop
offset="1"
style="stop-color:#959595;stop-opacity:1"
id="stop5132-4" />
</linearGradient>
<linearGradient
id="linearGradient3397-1">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-0" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-3" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-1"
id="linearGradient3328"
y2="47.044857"
x2="25.922546"
y1="19"
x1="25.922546" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-8"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-4" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313-8" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-8"
id="linearGradient3326"
y2="47.012184"
x2="32.036148"
y1="19"
x1="32.036148" />
<linearGradient
id="linearGradient10691">
<stop
offset="0"
style="stop-color:#000000;stop-opacity:1"
id="stop10693" />
<stop
offset="1"
style="stop-color:#000000;stop-opacity:0"
id="stop10695" />
</linearGradient>
<radialGradient
gradientTransform="matrix(2.2134617,0,0,0.4842001,1.1638028,-7.1447362)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient10691"
id="radialGradient2538"
fy="73.615715"
fx="6.702713"
r="7.228416"
cy="73.615715"
cx="6.702713" />
<linearGradient
gradientTransform="matrix(0.3244803,0,0,0.3244803,3.2332655,2.5577454)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128"
id="linearGradient2512"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient5128">
<stop
offset="0"
style="stop-color:#e5e5e5;stop-opacity:1"
id="stop5130" />
<stop
offset="1"
style="stop-color:#ababab;stop-opacity:1"
id="stop5132" />
</linearGradient>
<linearGradient
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309"
id="linearGradient2515"
y2="45.017357"
x2="20.758585"
y1="1"
x1="20.758585" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313" />
</linearGradient>
<linearGradient
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397"
id="linearGradient2517"
y2="45"
x2="37.201294"
y1="1"
x1="37.201294" />
<linearGradient
id="linearGradient3397">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401" />
</linearGradient>
<radialGradient
gradientTransform="matrix(2.1647059,0,0,0.7529402,-111.56471,36.517647)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient8838-9"
id="radialGradient2436-5"
fy="4.625"
fx="62.625"
r="10.625"
cy="4.625"
cx="62.625" />
<linearGradient
id="linearGradient8838-9">
<stop
offset="0"
style="stop-color:#000000;stop-opacity:1"
id="stop8840-7" />
<stop
offset="1"
style="stop-color:#000000;stop-opacity:0"
id="stop8842-8" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3309-8-1"
id="linearGradient3326-4"
y2="47.012184"
x2="32.036148"
y1="19"
x1="32.036148" />
<linearGradient
gradientUnits="userSpaceOnUse"
id="linearGradient3309-8-1"
y2="122.8994"
x2="63.9995"
y1="3.1001"
x1="63.9995">
<stop
offset="0"
style="stop-color:#f6f6f6;stop-opacity:1"
id="stop3311-4-5" />
<stop
offset="1"
style="stop-color:#d2d2d2;stop-opacity:1"
id="stop3313-8-5" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.93928017,0.34315123,-0.34315123,0.93928017,-1.7936946,-25.633934)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient3397-1-9"
id="linearGradient3328-4"
y2="47.044857"
x2="25.922546"
y1="19"
x1="25.922546" />
<linearGradient
id="linearGradient3397-1-9">
<stop
offset="0"
style="stop-color:#aaaaaa;stop-opacity:1"
id="stop3399-0-8" />
<stop
offset="1"
style="stop-color:#8c8c8c;stop-opacity:1"
id="stop3401-3-3" />
</linearGradient>
<linearGradient
gradientTransform="matrix(0.2014014,0,0,0.2014015,3.1103045,3.311705)"
gradientUnits="userSpaceOnUse"
xlink:href="#linearGradient5128-0-5"
id="linearGradient2868-8"
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919" />
<linearGradient
id="linearGradient5128-0-5">
<stop
offset="0"
style="stop-color:#ffffff;stop-opacity:1"
id="stop5130-4-2" />
<stop
offset="1"
style="stop-color:#959595;stop-opacity:1"
id="stop5132-4-2" />
</linearGradient>
<linearGradient
y2="20.895"
x2="84.63858"
y1="105.105"
x1="86.132919"
gradientTransform="matrix(0.2014014,0,0,0.2014015,3.1103045,3.311705)"
gradientUnits="userSpaceOnUse"
id="linearGradient3339"
xlink:href="#linearGradient5128-0-5"
inkscape:collect="always" />
</defs>
<sodipodi:namedview
id="base"
@ -836,14 +1232,14 @@
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="1.4"
inkscape:cx="44.030427"
inkscape:cy="102.26308"
inkscape:zoom="1.979899"
inkscape:cx="69.908261"
inkscape:cy="99.737699"
inkscape:document-units="px"
inkscape:current-layer="layer1"
inkscape:current-layer="background"
showgrid="false"
inkscape:window-width="1366"
inkscape:window-height="693"
inkscape:window-height="691"
inkscape:window-x="0"
inkscape:window-y="24"
inkscape:window-maximized="1"
@ -861,7 +1257,7 @@
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
<dc:title />
<dc:creator>
<cc:Agent>
<dc:title>Edouard Lafargue</dc:title>
@ -977,7 +1373,7 @@
d="m 53.950159,30.352861 c 0,13.351915 -10.823865,24.17578 -24.175779,24.17578 -13.351915,0 -24.1757796,-10.823865 -24.1757796,-24.17578 0,-13.351914 10.8238646,-24.1757789 24.1757796,-24.1757789 13.351914,0 24.175779,10.8238649 24.175779,24.1757789 z" />
</g>
<g
id="symbol-False"
id="symbol-Disarmed"
transform="translate(173.85714,-69.957837)"
inkscape:label="#layer1-6">
<path
@ -998,7 +1394,7 @@
d="M 17.049914,14.239243 33.752151,33.92599" />
</g>
<g
id="symbol-True"
id="symbol-Armed"
transform="translate(92.42857,-72.302217)"
inkscape:label="#layer1-0">
<path
@ -1015,5 +1411,26 @@
id="path1544"
d="M 32.938536,7.2012775 26.730268,18.60791 21.197213,28.668822 c -0.178482,0.181096 -0.123845,0.754094 -0.902701,0.952966 -0.538869,0.137595 -0.636352,-0.06289 -1.204172,-0.427914 l -6.950556,-5.492153 c -0.7733,-0.621665 -0.749753,-0.589365 -1.297288,-0.03737 L 8.0515824,27.18582 c -0.7518117,0.804672 -0.6353589,1.290849 0.3017539,1.891047 0,0 13.0781537,10.780055 13.0949407,10.788373 0.155564,0.085 0.0892,0.114145 0.757862,0.57814 0.428927,0.297637 0.807539,-0.647982 1.083435,-1.15286 L 39.190779,10.274963 C 39.55113,9.6155282 39.523767,9.624421 38.860008,9.2617466 L 34.11801,6.7472827 C 33.434075,6.3758505 33.430508,6.3800895 32.938536,7.2012775 z" />
</g>
<g
style="display:inline"
id="symbol-Arming"
transform="matrix(1.2857213,0,0,1.2857213,18.29593,-68.120293)"
inkscape:label="#layer1-33">
<path
style="fill:url(#linearGradient3326-4);fill-opacity:1;fill-rule:nonzero;stroke:url(#linearGradient3328-4);stroke-width:0.77777356;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0;marker:none;visibility:visible;display:block;overflow:visible;enable-background:accumulate"
id="rect2576"
d="m 18.636572,2.5905216 c -0.703275,-0.2569307 -1.495689,0.08705 -1.752619,0.7903248 l -0.600515,1.6437399 c -0.499176,-0.013219 -0.993955,0.00248 -1.484533,0.056512 L 14.072373,3.5181351 C 13.756758,2.8391668 12.960838,2.5483897 12.28187,2.864003 L 8.3651471,4.6602954 C 7.6861782,4.9759083 7.3954033,5.7718319 7.7110151,6.4507982 l 0.726532,1.5629636 C 8.0724464,8.359396 7.744193,8.7441155 7.4266456,9.141602 L 5.8122578,8.5518108 C 5.1089821,8.2948801 4.3165666,8.6388602 4.0596381,9.3421356 L 2.5797984,13.392781 c -0.2569313,0.703275 0.1271272,1.477063 0.8304007,1.733991 l 1.6143878,0.589791 c -0.013469,0.508577 -0.010523,1.014295 0.045789,1.513885 l -1.5629633,0.726533 c -0.6789688,0.315613 -0.9697472,1.111534 -0.654132,1.790502 l 1.8363684,3.898094 c 0.3156123,0.678968 1.1115366,0.969744 1.7905029,0.654132 l 1.5629636,-0.726532 c 0.3402133,0.357549 0.7083491,0.688501 1.0984877,1.000178 l -0.6005147,1.64374 c -0.2569313,0.703275 0.1271291,1.477064 0.8304008,1.733991 l 4.0212927,1.469116 c 0.703276,0.256931 1.466338,-0.09777 1.723267,-0.801048 l 0.600515,-1.64374 c 0.508577,0.01347 1.014295,0.01052 1.513885,-0.04579 l 0.726532,1.562963 c 0.315614,0.678968 1.111534,0.969745 1.790502,0.654132 l 3.916723,-1.796292 c 0.678969,-0.315613 0.969742,-1.111538 0.654132,-1.790503 l -0.745161,-1.603039 c 0.357549,-0.340214 0.6885,-0.708349 1.000178,-1.098488 l 1.64374,0.600515 c 0.703276,0.25693 1.466338,-0.09778 1.723267,-0.801049 l 1.47984,-4.050645 c 0.256931,-0.703275 -0.09778,-1.466339 -0.801048,-1.723268 l -1.64374,-0.600514 c 0.01322,-0.499176 -0.0025,-0.993955 -0.05651,-1.484533 l 1.60304,-0.745162 C 29.20091,13.73813 29.491686,12.942208 29.176073,12.26324 L 27.339704,8.365146 C 27.024092,7.6861784 26.228168,7.3954 25.549202,7.711014 L 23.986239,8.4375465 C 23.640605,8.0724458 23.255886,7.7441924 22.858399,7.4266449 L 23.458914,5.782905 C 23.715845,5.0796306 23.361136,4.3165668 22.657865,4.0596378 L 18.636572,2.5905216 z M 17.20103,12.71252 c 1.81469,0.662968 2.74942,2.67382 2.086452,4.48851 -0.662969,1.814689 -2.673821,2.749419 -4.48851,2.086451 -1.814691,-0.662969 -2.74942,-2.673821 -2.086452,-4.48851 0.662968,-1.81469 2.673821,-2.74942 4.48851,-2.086451 z"
inkscape:connector-curvature="0" />
<path
style="opacity:0.05;fill:#000000;fill-opacity:1;stroke:none"
id="path3315"
d="m 16.000001,7.9999996 c -4.411273,0 -7.9999985,3.5887274 -7.9999985,8.0000004 0,4.411274 3.5887255,8 7.9999985,8 C 20.411274,24 24,20.411274 24,16 24,11.588727 20.411274,7.9999996 16.000001,7.9999996 z m 0,3.6923094 c 2.377844,0 4.307693,1.929846 4.307693,4.307691 0,2.377847 -1.929849,4.307693 -4.307693,4.307693 -2.377846,0 -4.307691,-1.929846 -4.307691,-4.307693 0,-2.377845 1.929845,-4.307691 4.307691,-4.307691 z"
inkscape:connector-curvature="0" />
<path
style="fill:none;stroke:url(#linearGradient3339);stroke-width:0.77777356;stroke-miterlimit:4;stroke-dasharray:none"
id="path28"
d="m 16.000001,7.6206906 c -4.620353,0 -8.3793085,3.7589574 -8.3793085,8.3793084 0,4.620352 3.7589555,8.37931 8.3793085,8.37931 4.620353,0 8.379309,-3.758958 8.379309,-8.37931 0,-4.620351 -3.758956,-8.3793084 -8.379309,-8.3793084 z"
inkscape:connector-curvature="0" />
</g>
</g>
</svg>

Before

Width:  |  Height:  |  Size: 37 KiB

After

Width:  |  Height:  |  Size: 52 KiB

View File

@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>400</width>
<height>307</height>
<height>343</height>
</rect>
</property>
<property name="windowTitle">
@ -142,6 +142,18 @@
</widget>
</widget>
</item>
<item>
<widget class="QCheckBox" name="zeroGyroBiasOnArming">
<property name="toolTip">
<string>If enabled, a fast recalibration of gyro zero point will be done
whenever the frame is armed. Do not move the airframe while
arming it in that case!</string>
</property>
<property name="text">
<string>Zero gyro bias upon airframe arming</string>
</property>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="sizePolicy">
@ -241,6 +253,19 @@
</widget>
</widget>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>

View File

@ -53,7 +53,6 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget()
void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) {
QMutexLocker locker(&startStop);
UAVDataObject * attitudeRaw = dynamic_cast<UAVDataObject*>(obj);
ui->zeroBiasProgress->setValue((float) updates / NUM_ACCEL_UPDATES * 100);
@ -94,7 +93,6 @@ void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) {
void ConfigCCAttitudeWidget::timeout() {
QMutexLocker locker(&startStop);
UAVObjectManager * objMngr = getObjectManager();
UAVDataObject * obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*)));
disconnect(&timer,SIGNAL(timeout()),this,SLOT(timeout()));
@ -115,6 +113,11 @@ void ConfigCCAttitudeWidget::applyAttitudeSettings() {
field->setValue(ui->pitchBias->value(),1);
field->setValue(ui->yawBias->value(),2);
field = settings->getField("ZeroDuringArming");
// Handling of boolean values is done through enums on
// uavobjects...
field->setValue((ui->zeroGyroBiasOnArming->isChecked()) ? "TRUE": "FALSE");
settings->updated();
}
@ -125,6 +128,12 @@ void ConfigCCAttitudeWidget::getCurrentAttitudeSettings() {
ui->rollBias->setValue(field->getDouble(0));
ui->pitchBias->setValue(field->getDouble(1));
ui->yawBias->setValue(field->getDouble(2));
field = settings->getField("ZeroDuringArming");
// Handling of boolean values is done through enums on
// uavobjects...
bool enabled = (field->getValue().toString() == "FALSE") ? false : true;
ui->zeroGyroBiasOnArming->setChecked(enabled);
}
void ConfigCCAttitudeWidget::startAccelCalibration() {

View File

@ -1138,7 +1138,7 @@ LineardialGadget\Accel%20Horizontal%20Z\data\useOpenGLFlag=false
LineardialGadget\Accel%20Horizontal%20Z\configInfo\version=0.0.0
LineardialGadget\Accel%20Horizontal%20Z\configInfo\locked=false
LineardialGadget\Arm%20Status\data\dFile=%%DATAPATH%%dials/default/arm-status.svg
LineardialGadget\Arm%20Status\data\sourceDataObject=ManualControlCommand
LineardialGadget\Arm%20Status\data\sourceDataObject=FlightStatus
LineardialGadget\Arm%20Status\data\sourceObjectField=Armed
LineardialGadget\Arm%20Status\data\minValue=0
LineardialGadget\Arm%20Status\data\maxValue=100
@ -1172,7 +1172,7 @@ LineardialGadget\Flight%20Time\data\useOpenGLFlag=false
LineardialGadget\Flight%20Time\configInfo\version=0.0.0
LineardialGadget\Flight%20Time\configInfo\locked=false
LineardialGadget\Flight%20mode\data\dFile=%%DATAPATH%%dials/default/flightmode-status.svg
LineardialGadget\Flight%20mode\data\sourceDataObject=ManualControlCommand
LineardialGadget\Flight%20mode\data\sourceDataObject=FlightStatus
LineardialGadget\Flight%20mode\data\sourceObjectField=FlightMode
LineardialGadget\Flight%20mode\data\minValue=0
LineardialGadget\Flight%20mode\data\maxValue=100

View File

@ -249,13 +249,17 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
case 1://Armed
if (currentCGSControl)
{
if(obj->getField("Armed")->getValue().toString().compare("True")==0)
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
if(obj->getField("Armed")->getValue().toString().compare("Armed")==0)
{
obj->getField("Armed")->setValue("False");
obj->getField("Armed")->setValue("Disarmed");
}
else
{
obj->getField("Armed")->setValue("True");
obj->getField("Armed")->setValue("Armed");
}
}
break;

View File

@ -52,7 +52,8 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
m_gcscontrol->checkBoxGcsControl->setChecked(mdata.flightAccess == UAVObject::ACCESS_READONLY);
// Set up the drop down box for the flightmode
m_gcscontrol->comboBoxFlightMode->addItems(obj->getField("FlightMode")->getOptions());
UAVDataObject* flightStatus = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions());
// Set up slots and signals for joysticks
connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double)));
@ -133,18 +134,21 @@ void GCSControlGadgetWidget::toggleArmed(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
if(state)
obj->getField("Armed")->setValue("True");
obj->getField("Armed")->setValue("Armed");
else
obj->getField("Armed")->setValue("False");
obj->getField("Armed")->setValue("Disarmed");
obj->updated();
}
void GCSControlGadgetWidget::mccChanged(UAVObject * obj)
{
m_gcscontrol->checkBoxArmed->setChecked(obj->getField("Armed")->getValue() == "True");
m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(obj->getField("FlightMode")->getValue().toString()));
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* flightStatus = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
m_gcscontrol->comboBoxFlightMode->setCurrentIndex(m_gcscontrol->comboBoxFlightMode->findText(flightStatus->getField("FlightMode")->getValue().toString()));
m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
}
/*!
@ -154,7 +158,7 @@ void GCSControlGadgetWidget::selectFlightMode(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
UAVObjectField * field = obj->getField("FlightMode");
field->setValue(field->getOptions()[state]);
obj->updated();

View File

@ -155,17 +155,17 @@ void FGSimulator::processReadyRead()
void FGSimulator::transmitUpdate()
{
ActuatorDesired::DataFields actData;
FlightStatus::DataFields flightStatusData = flightStatus->getData();
ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
float ailerons = -1;
float elevator = -1;
float rudder = -1;
float throttle = -1;
if(manCtrlData.FlightMode == ManualControlCommand::FLIGHTMODE_MANUAL)
if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL)
{
// Read joystick input
if(manCtrlData.Armed == ManualControlCommand::ARMED_TRUE)
if(flightStatusData.Armed == FlightStatus::ARMED_ARMED)
{
// Note: Pitch sign is reversed in FG ?
ailerons = manCtrlData.Roll;

View File

@ -125,6 +125,7 @@ void Simulator::onStart()
UAVObjectManager* objManager = pm->getObject<UAVObjectManager>();
actDesired = ActuatorDesired::GetInstance(objManager);
manCtrlCommand = ManualControlCommand::GetInstance(objManager);
flightStatus = FlightStatus::GetInstance(objManager);
posHome = HomeLocation::GetInstance(objManager);
velActual = VelocityActual::GetInstance(objManager);
posActual = PositionActual::GetInstance(objManager);

View File

@ -46,6 +46,7 @@
#include "homelocation.h"
#include "attituderaw.h"
#include "gcstelemetrystats.h"
#include "flightstatus.h"
#include "utils/coordinateconversions.h"
@ -170,6 +171,7 @@ protected:
ActuatorDesired* actDesired;
ManualControlCommand* manCtrlCommand;
FlightStatus* flightStatus;
BaroAltitude* altActual;
AttitudeActual* attActual;
VelocityActual* velActual;

View File

@ -66,6 +66,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/ahrsstatus.h \
$$UAVOBJECT_SYNTHETICS/watchdogstatus.h \
$$UAVOBJECT_SYNTHETICS/nedaccel.h \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.h \
$$UAVOBJECT_SYNTHETICS/flightstatus.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
@ -112,4 +113,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
$$UAVOBJECT_SYNTHETICS/nedaccel.cpp \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \
$$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \
$$UAVOBJECT_SYNTHETICS/flightstatus.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp

View File

@ -7,6 +7,7 @@
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.01"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

View File

@ -0,0 +1,14 @@
<xml>
<object name="FlightStatus" singleinstance="true" settings="false">
<description>Contains major flight status information for other modules.</description>
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,VelocityControl,PositionHold"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="5000"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

View File

@ -2,15 +2,11 @@
<object name="ManualControlCommand" singleinstance="true" settings="false">
<description>The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control.</description>
<field name="Connected" units="" type="enum" elements="1" options="False,True"/>
<field name="Armed" units="" type="enum" elements="1" options="False,True"/>
<field name="Roll" units="%" type="float" elements="1"/>
<field name="Pitch" units="%" type="float" elements="1"/>
<field name="Yaw" units="%" type="float" elements="1"/>
<field name="Throttle" units="%" type="float" elements="1"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,VelocityControl,PositionHold"/>
<field name="Accessory1" units="%" type="float" elements="1"/>
<field name="Accessory2" units="%" type="float" elements="1"/>
<field name="Accessory3" units="%" type="float" elements="1"/>