rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1309 some small fixes to Stabilization as result of simulator testing

Browse Source
This commit is contained in:
Corvus Corax 2014-04-29 15:51:56 +02:00
parent a9a2c6f769
commit 1632eeb58f
4 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/libraries/sanitycheck.c
View File

@ -239,6 +239,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) {
return SYSTEMALARMS_ALARM_ERROR;
}

20
flight/modules/Stabilization/innerloop.c
View File

@ -109,39 +109,41 @@ static void stabilizationInnerloopTask()
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
uint8_t errorlevel = 0;
bool warn = false;
bool error = false;
bool crit = false;
// check if outer loop keeps executing
if (stabSettings.monitor.rateupdates > -64) {
stabSettings.monitor.rateupdates--;
}
if (stabSettings.monitor.rateupdates < -3) {
// warning if rate loop skipped more than 2 beats
errorlevel |= 1;
warn = true;
}
if (stabSettings.monitor.rateupdates < -7) {
// error if rate loop skipped more than 7 beats
errorlevel |= 2;
error = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// critical if gyro didn't update at all!
errorlevel |= 4;
crit = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
errorlevel |= 1;
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// error if we missed 3 gyro updates
errorlevel |= 2;
error = true;
}
stabSettings.monitor.gyroupdates = 0;
if (errorlevel & 4) {
if (crit) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else if (errorlevel & 2) {
} else if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else if (errorlevel & 1) {
} else if (warn) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);

2
flight/modules/Stabilization/outerloop.c
View File

@ -98,6 +98,7 @@ static void stabilizationOuterloopTask()
StabilizationDesiredData stabilizationDesired;
StabilizationStatusOuterLoopData enabled;
AttitudeStateGet(&attitudeState);
StabilizationDesiredGet(&stabilizationDesired);
RateDesiredGet(&rateDesired);
StabilizationStatusOuterLoopGet(&enabled);
@ -139,7 +140,6 @@ static void stabilizationOuterloopTask()
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];

1
shared/uavobjectdefinition/ratedesired.xml
View File

@ -4,6 +4,7 @@
<field name="Roll" units="deg/s" type="float" elements="1"/>
<field name="Pitch" units="deg/s" type="float" elements="1"/>
<field name="Yaw" units="deg/s" type="float" elements="1"/>
<field name="Thrust" units="%" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>