rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +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 if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) { )) {
return SYSTEMALARMS_ALARM_ERROR; return SYSTEMALARMS_ALARM_ERROR;
} }

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

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

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

@ -98,6 +98,7 @@ static void stabilizationOuterloopTask()
StabilizationDesiredData stabilizationDesired; StabilizationDesiredData stabilizationDesired;
StabilizationStatusOuterLoopData enabled; StabilizationStatusOuterLoopData enabled;
AttitudeStateGet(&attitudeState);
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
StabilizationStatusOuterLoopGet(&enabled); StabilizationStatusOuterLoopGet(&enabled);
@ -139,7 +140,6 @@ static void stabilizationOuterloopTask()
} }
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
} }
for (t = 0; t < AXES; t++) { for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[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]; 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="Roll" units="deg/s" type="float" elements="1"/>
<field name="Pitch" 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="Yaw" units="deg/s" type="float" elements="1"/>
<field name="Thrust" units="%" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>