rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merged in f5soh/librepilot/LP-401_LP-411_Autotune (pull request #357)

Browse Source 
LP-401 and LP-411 Autotune
This commit is contained in:
Philippe Renon 2016-11-21 20:36:43 +00:00
commit 2e4d2371a3
2 changed files with 77 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
flight/modules/AutoTune/autotune.c
View File

@ -146,6 +146,7 @@ static bool moduleEnabled;
// Private functions
static void AtNewGyroData(UAVObjEvent *ev);
static bool AutoTuneFoundInFMS();
static void AutoTuneTask(void *parameters);
static void AfInit(float X[AF_NUMX], float P[AF_NUMP]);
static void AfPredict(float X[AF_NUMX], float P[AF_NUMP], const float u_in[3], const float gyro[3], const float dT_s, const float t_in);
@ -153,35 +154,28 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
static uint8_t CheckSettings();
static uint8_t CheckSettingsRaw();
static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
static void FlightModeSettingsUpdatedCb(UAVObjEvent *ev);
static void InitSystemIdent(bool loadDefaults);
static void ProportionPidsSmoothToQuick();
static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
static void UpdateStabilizationDesired(bool doingIdent);
static void flightModeSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
FlightModeSettingsFlightModePositionGet(fms);
for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
break;
}
}
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutoTuneInitialize(void)
{
// Create a queue, connect to manual control command and flightstatus
#ifdef MODULE_AutoTune_BUILTIN
// do this here since module can become disabled for several reasons
// even for MODULE_AutoTune_BUILTIN
FlightModeSettingsInitialize();
#if defined(MODULE_AutoTune_BUILTIN)
moduleEnabled = true;
#else
// HwSettings is only used right here, so init here
HwSettingsInitialize();
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
@ -191,32 +185,33 @@ int32_t AutoTuneInitialize(void)
// that allows PIDs to be adjusted in flight
moduleEnabled = true;
} else {
// if the user did not enable the autotune module
// if the user did not manually enable the autotune module
// do it for them if they have autotune on their flight mode switch
FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
moduleEnabled = false;
FlightModeSettingsInitialize();
FlightModeSettingsFlightModePositionGet(fms);
for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
moduleEnabled = true;
break;
}
}
moduleEnabled = AutoTuneFoundInFMS();
}
#endif /* ifdef MODULE_AutoTune_BUILTIN */
#endif /* defined(MODULE_AutoTune_BUILTIN) */
if (moduleEnabled) {
AccessoryDesiredInitialize();
ActuatorDesiredInitialize();
FlightStatusInitialize();
GyroStateInitialize();
ManualControlCommandInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
SystemIdentSettingsInitialize();
SystemIdentStateInitialize();
atQueue = xQueueCreate(AT_QUEUE_NUMELEM, sizeof(struct at_queued_data));
if (!atQueue) {
moduleEnabled = false;
}
}
if (!moduleEnabled) {
FlightModeSettingsConnectCallback(flightModeSettingsUpdatedCb);
// only need to watch for enabling AutoTune in FMS if AutoTune module is _not_ running
FlightModeSettingsConnectCallback(FlightModeSettingsUpdatedCb);
}
return 0;
@ -577,6 +572,26 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
}
// FlightModeSettings callback
// determine if autotune is enabled in the flight mode switch
static bool AutoTuneFoundInFMS()
{
bool found = false;
FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
uint8_t num_flightMode;
FlightModeSettingsFlightModePositionGet(fms);
ManualControlSettingsFlightModeNumberGet(&num_flightMode);
for (uint8_t i = 0; i < num_flightMode; ++i) {
if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
found = true;
break;
}
}
return found;
}
// gyro sensor callback
// get gyro data and actuatordesired into a packet
// and put it in the queue for later processing
@ -624,6 +639,16 @@ static void AtNewGyroData(UAVObjEvent *ev)
}
// this callback is only enabled if the AutoTune module is not running
// if it sees that AutoTune was added to the FMS it issues BOOT and ? alarms
static void FlightModeSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (AutoTuneFoundInFMS()) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
}
}
// check for the user quickly toggling the flight mode switch
// into and out of AutoTune, 3 times
// that is a signal that the user wants to try the next PID settings
@ -925,11 +950,29 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
// inner loop as a single order lpf. Set the outer loop to be
// critically damped;
const float zeta_o = 1.3f;
const float kp_o = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f / wn);
const float ki_o = 0.75f * kp_o / (2.0f * M_PI_F * tau * 10.0f);
float kp_o = 1.0f / 4.0f / (zeta_o * zeta_o) / (1.0f / wn);
// Except, if this is very high, we may be slew rate limited and pick
// up oscillation that way. Fix it with very soft clamping.
// (dRonin) MaximumRate defaults to 350, 6.5 corresponds to where we begin
// clamping rate ourselves. ESCs, etc, it depends upon gains
// and any pre-emphasis they do. Still give ourselves partial credit
// for inner loop bandwidth.
// In dRonin, MaximumRate defaults to 350 and they begin clamping at outer Kp 6.5
// To avoid oscillation, find the minimum rate, calculate the ratio of that to 350,
// and scale (linearly) with that. Skip yaw. There is no outer yaw in the GUI.
const uint16_t minRate = MIN(stabSettingsBank.MaximumRate.Roll, stabSettingsBank.MaximumRate.Pitch);
const float kp_o_clamp = systemIdentSettings.OuterLoopKpSoftClamp * ((float)minRate / 350.0f);
if (kp_o > kp_o_clamp) {
kp_o = kp_o_clamp - sqrtf(kp_o_clamp) + sqrtf(kp_o);
}
kp_o *= 0.95f; // Pick up some margin.
// Add a zero at 1/15th the innermost bandwidth.
const float ki_o = 0.75f * kp_o / (2.0f * M_PI_F * tau * 15.0f);
float kpMax = 0.0f;
float betaMinLn = 1000.0f;
float betaMinLn = 1000.0f;
StabilizationBankRollRatePIDData volatile *rollPitchPid = NULL; // satisfy compiler warning only
for (int i = 0; i < ((systemIdentSettings.CalculateYaw != SYSTEMIDENTSETTINGS_CALCULATEYAW_FALSE) ? 3 : 2); i++) {

3
shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -54,7 +54,8 @@
<!-- Cliff sluggish 500 quad thinks that yaw P should be about 5.8 times roll/pitch P, but can easily (and better) live with 2.6 -->
<field name="YawToRollPitchPIDRatioMin" units="" type="float" elements="1" defaultvalue="1.0" description="Setting: Yaw PID will be at least this times Pitch PID (if enabled)"/>
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5" description="Setting: Yaw PID will be at most this times Pitch PID (if enabled)"/>
<field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1" description="Setting: Reduce this toward zero if you have D term oscillations, it will reduce PID D terms"/>
<field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1" description="Setting: Multiplicative factor. If you have D term oscillations, reduce it toward zero and it will reduce PID D terms."/>
<field name="OuterLoopKpSoftClamp" units="" type="float" elements="1" defaultvalue="6.5" limits="%BE:0:100" description="Setting: Change this to change the outer loop Kp and Ki limiting"/>
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="3" limits="%BE:1:3" description="Setting: Which bank the calculated PIDs will be stored in after tuning"/>
<field name="TuningDuration" units="s" type="uint8" elements="1" defaultvalue="60" limits="%BI:0" description="Setting: Duration of the tuning motions (expert)"/>
<!-- SmoothQuickSource: the smooth vs. quick PID selector -->