1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00

LP-76 disable stabHandler for AutoTune mode

This commit is contained in:
Cliff Geerdes 2016-03-07 21:57:29 -05:00
parent faf2fffb5a
commit 78dffae5a1
2 changed files with 15 additions and 2 deletions

View File

@ -798,7 +798,8 @@ static void UpdateSystemIdent(const float *X, const float *noise,
static void UpdateStabilizationDesired(bool doingIdent) {
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// unneeded since we are setting everything in this uavo
//StabilizationDesiredGet(&stabDesired);
ManualControlCommandData manual_control_command;
ManualControlCommandGet(&manual_control_command);
@ -806,7 +807,8 @@ static void UpdateStabilizationDesired(bool doingIdent) {
stabDesired.Roll = manual_control_command.Roll * rollMax;
stabDesired.Pitch = manual_control_command.Pitch * pitchMax;
stabDesired.Yaw = manual_control_command.Yaw * manualRate.Yaw;
stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle;
//stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle;
stabDesired.Thrust = manual_control_command.Thrust;
if (doingIdent) {
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT;
@ -829,6 +831,7 @@ static uint8_t CheckSettings()
{
uint8_t retVal = 0;
#if 1
// Check the axis gains
// Extreme values: Your roll or pitch gain was lower than expected. This will result in large PID values.
if (systemIdentData.Beta.Roll < 6) {
@ -847,6 +850,7 @@ static uint8_t CheckSettings()
else if (expf(systemIdentData.Tau) < 0.008f) {
retVal |= 8;
}
#endif
return retVal;
}

View File

@ -49,6 +49,7 @@ static float applyExpo(float value, float expo);
static uint8_t currentFpvTiltAngle = 0;
static float cosAngle = 0.0f;
static float sinAngle = 0.0f;
#if 0
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
static FlightModeSettingsStabilization1SettingsData autotuneSettings = {
.Roll = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT,
@ -65,6 +66,7 @@ static FlightModeSettingsStabilization1SettingsData attitudeSettings = {
};
#endif
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
#endif
static float applyExpo(float value, float expo)
{
@ -100,6 +102,7 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
StabilizationDesiredInitialize();
StabilizationBankInitialize();
}
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
@ -157,12 +160,18 @@ void stabilizedHandler(__attribute__((unused)) bool newinit)
break;
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
#if 0
// if (SystemIdentHandle()) {
stab_settings = (uint8_t *)&autotuneSettings;
// } else {
// stab_settings = (uint8_t *)&attitudeSettings;
// }
break;
#else
// let autotune.c handle it
// because it must switch to Attitude after <user configurable> seconds
return;
#endif
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
default:
// Major error, this should not occur because only enter this block when one of these is true