rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

LP-340 cosmetic type of issues requested in PR

Browse Source
This commit is contained in:
Cliff Geerdes 2016-07-19 19:04:40 -04:00
parent d4f3e7e602
commit 447dff6fc9
4 changed files with 33 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -134,7 +134,7 @@ static float gyroReadTimeAverage;
static float gyroReadTimeAverageAlpha; static float gyroReadTimeAverageAlpha;
static float gyroReadTimeAverageAlphaAlpha; static float gyroReadTimeAverageAlphaAlpha;
static float alpha; static float alpha;
static float smoothQuickSetting; static float smoothQuickValue;
static volatile uint32_t atPointsSpilled; static volatile uint32_t atPointsSpilled;
static uint32_t throttleAccumulator; static uint32_t throttleAccumulator;
static uint8_t rollMax, pitchMax; static uint8_t rollMax, pitchMax;
@ -232,7 +232,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
float noise[3] = { 0 }; float noise[3] = { 0 };
float dT_s = 0.0f; float dT_s = 0.0f;
uint32_t lastUpdateTime = 0; // initialization is only for compiler warning uint32_t lastUpdateTime = 0; // initialization is only for compiler warning
uint32_t lastTime = 0.0f; uint32_t lastTime = 0;
uint32_t measureTime = 0; uint32_t measureTime = 0;
uint32_t updateCounter = 0; uint32_t updateCounter = 0;
enum AUTOTUNE_STATE state = AT_INIT; enum AUTOTUNE_STATE state = AT_INIT;
@ -254,7 +254,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// based on what is in SystemIdent // based on what is in SystemIdent
// so that the user can use the PID smooth->quick slider in flights following the autotune flight // so that the user can use the PID smooth->quick slider in flights following the autotune flight
InitSystemIdent(false); InitSystemIdent(false);
smoothQuickSetting = systemIdentSettings.SmoothQuickSetting; smoothQuickValue = systemIdentSettings.SmoothQuickValue;
while (1) { while (1) {
uint32_t diffTime; uint32_t diffTime;
@ -297,15 +297,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// if user toggled while armed set PID's to next in sequence // if user toggled while armed set PID's to next in sequence
// if you assume that smoothest is -1 and quickest is +1 // if you assume that smoothest is -1 and quickest is +1
// this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle) // this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle)
smoothQuickSetting += 1.0f / (float)flightModeSwitchTogglePosition; smoothQuickValue += 1.0f / (float)flightModeSwitchTogglePosition;
if (smoothQuickSetting > 1.001f) { if (smoothQuickValue > 1.001f) {
smoothQuickSetting = -1.0f; smoothQuickValue = -1.0f;
} }
} else { } else {
// if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick // if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick
smoothQuickSetting = 0.0f; smoothQuickValue = 0.0f;
} }
// calculate PIDs based on new smoothQuickSetting and save to the PID bank // calculate PIDs based on new smoothQuickValue and save to the PID bank
ProportionPidsSmoothToQuick(); ProportionPidsSmoothToQuick();
// save new PIDs permanently when / if disarmed // save new PIDs permanently when / if disarmed
savePidNeeded = true; savePidNeeded = true;
@ -347,9 +347,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0 // a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0
// what we are doing here does not need any higher precision than that // what we are doing here does not need any higher precision than that
// user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed // user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed
if (fabsf(smoothQuickSetting - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) { if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) {
smoothQuickSetting = accessoryValue.AccessoryVal; smoothQuickValue = accessoryValue.AccessoryVal;
// calculate PIDs based on new smoothQuickSetting and save to the PID bank // calculate PIDs based on new smoothQuickValue and save to the PID bank
ProportionPidsSmoothToQuick(); ProportionPidsSmoothToQuick();
// this schedules the first possible write of the PIDs to occur a fraction of a second or so from now // this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
// and changes the scheduled time if it is already scheduled // and changes the scheduled time if it is already scheduled
@ -432,7 +432,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
break; break;
case AT_START: case AT_START:
diffTime = xTaskGetTickCount() - lastUpdateTime; diffTime = xTaskGetTickCount() - lastUpdateTime;
doingIdent = true; doingIdent = true;
// after an additional short delay, start capturing data // after an additional short delay, start capturing data
if (diffTime > INIT_TIME_DELAY2_MS) { if (diffTime > INIT_TIME_DELAY2_MS) {
@ -443,9 +443,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// and the complete data has been sanity checked // and the complete data has been sanity checked
savePidNeeded = false; savePidNeeded = false;
// get the tuning duration in case the user just changed it // get the tuning duration in case the user just changed it
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000; measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
// init the "previous packet timestamp" // init the "previous packet timestamp"
lastTime = PIOS_DELAY_GetRaw(); lastTime = PIOS_DELAY_GetRaw();
/* Drain the queue of all current data */ /* Drain the queue of all current data */
xQueueReset(atQueue); xQueueReset(atQueue);
/* And reset the point spill counter */ /* And reset the point spill counter */
@ -478,8 +478,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.gyroStateCallbackTimestamp) * 1.0e-6f; dT_s = PIOS_DELAY_DiffuS2(lastTime, pt.gyroStateCallbackTimestamp) * 1.0e-6f;
/* This is for the first point, but /* This is for the first point, but
* also if we have extended drops */ * also if we have extended drops */
if (dT_s > 5.0f/PIOS_SENSOR_RATE) { if (dT_s > 5.0f / PIOS_SENSOR_RATE) {
dT_s = 5.0f/PIOS_SENSOR_RATE; dT_s = 5.0f / PIOS_SENSOR_RATE;
} }
lastTime = pt.gyroStateCallbackTimestamp; lastTime = pt.gyroStateCallbackTimestamp;
// original algorithm handles time from GyroStateGet() to detected motion // original algorithm handles time from GyroStateGet() to detected motion
@ -690,7 +690,7 @@ static void InitSystemIdent(bool loadDefaults)
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
// leave smoothQuickSetting alone since it is always controlled by knob // leave smoothQuickValue alone since it is always controlled by knob
// disable PID changing with flight mode switch // disable PID changing with flight mode switch
flightModeSwitchTogglePosition = -1; flightModeSwitchTogglePosition = -1;
// enable PID changing with accessory0-3 // enable PID changing with accessory0-3
@ -702,7 +702,7 @@ static void InitSystemIdent(bool loadDefaults)
// don't allow init of current toggle position in the middle of 3x fms toggle // don't allow init of current toggle position in the middle of 3x fms toggle
if (loadDefaults) { if (loadDefaults) {
// set toggle to middle of range // set toggle to middle of range
smoothQuickSetting = 0.0f; smoothQuickValue = 0.0f;
} }
// enable PID changing with flight mode switch // enable PID changing with flight mode switch
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
@ -711,7 +711,7 @@ static void InitSystemIdent(bool loadDefaults)
break; break;
case SMOOTH_QUICK_DISABLED: case SMOOTH_QUICK_DISABLED:
default: default:
// leave smoothQuickSetting alone so user can set it to a different value and have it stay that value // leave smoothQuickValue alone so user can set it to a different value and have it stay that value
// disable PID changing with flight mode switch // disable PID changing with flight mode switch
flightModeSwitchTogglePosition = -1; flightModeSwitchTogglePosition = -1;
// disable PID changing with accessory0-3 // disable PID changing with accessory0-3
@ -751,7 +751,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise,
systemIdentSettings.Tau = u.systemIdentState.Tau; systemIdentSettings.Tau = u.systemIdentState.Tau;
memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); memcpy(&systemIdentSettings.Beta, &u.systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; systemIdentSettings.GyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage;
systemIdentSettings.SmoothQuickSetting = smoothQuickSetting; systemIdentSettings.SmoothQuickValue = smoothQuickValue;
SystemIdentStateSet(&u.systemIdentState); SystemIdentStateSet(&u.systemIdentState);
} }
@ -847,6 +847,7 @@ static uint8_t CheckSettingsRaw()
static uint8_t CheckSettings() static uint8_t CheckSettings()
{ {
uint8_t retVal = CheckSettingsRaw(); uint8_t retVal = CheckSettingsRaw();
if (systemIdentSettings.DisableSanityChecks) { if (systemIdentSettings.DisableSanityChecks) {
retVal = 0; retVal = 0;
} }
@ -1007,7 +1008,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
// the amount to reduce kp and ki is taken from ZN tuning // the amount to reduce kp and ki is taken from ZN tuning
// specifically kp is parameterized based on the ratio between kp(PID) and kp(PI) as the D factor varies from 1 to 0 // specifically kp is parameterized based on the ratio between kp(PID) and kp(PI) as the D factor varies from 1 to 0
// https://en.wikipedia.org/wiki/PID_controller // https://en.wikipedia.org/wiki/PID_controller
// Kp Ki Kd // Kp Ki Kd
// ----------------------------------- // -----------------------------------
// P 0.50*Ku - - // P 0.50*Ku - -
// PI 0.45*Ku 1.2*Kp/Tu - // PI 0.45*Ku 1.2*Kp/Tu -
@ -1015,13 +1016,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
// //
// so Kp is multiplied by (.45/.60) if Kd is reduced to 0 // so Kp is multiplied by (.45/.60) if Kd is reduced to 0
// and Ki is multiplied by (1.2/2.0) if Kd is reduced to 0 // and Ki is multiplied by (1.2/2.0) if Kd is reduced to 0
#define KP_REDUCTION (.45f/.60f) #define KP_REDUCTION (.45f / .60f)
#define KI_REDUCTION (1.2f/2.0f) #define KI_REDUCTION (1.2f / 2.0f)
// this link gives some additional ratios that are different // this link gives some additional ratios that are different
// the reduced overshoot ratios are invalid for this purpose // the reduced overshoot ratios are invalid for this purpose
// https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
// Kp Ki Kd // Kp Ki Kd
// ------------------------------------------------ // ------------------------------------------------
// P 0.50*Ku - - // P 0.50*Ku - -
// PI 0.45*Ku Tu/1.2 - // PI 0.45*Ku Tu/1.2 -
@ -1034,8 +1035,8 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
// reduce roll and pitch, but not yaw // reduce roll and pitch, but not yaw
// yaw PID is entirely based on roll or pitch PIDs which have already been reduced // yaw PID is entirely based on roll or pitch PIDs which have already been reduced
if (i < 2) { if (i < 2) {
kp = kp * KP_REDUCTION + kp * systemIdentSettings.DerivativeFactor * (1.0f-KP_REDUCTION); kp = kp * KP_REDUCTION + kp * systemIdentSettings.DerivativeFactor * (1.0f - KP_REDUCTION);
ki = ki * KI_REDUCTION + ki * systemIdentSettings.DerivativeFactor * (1.0f-KI_REDUCTION); ki = ki * KI_REDUCTION + ki * systemIdentSettings.DerivativeFactor * (1.0f - KI_REDUCTION);
kd *= systemIdentSettings.DerivativeFactor; kd *= systemIdentSettings.DerivativeFactor;
} }
@ -1105,7 +1106,7 @@ static void ProportionPidsSmoothToQuick()
{ {
float ratio, damp, noise; float ratio, damp, noise;
float min = -1.0f; float min = -1.0f;
float val = smoothQuickSetting; float val = smoothQuickValue;
float max = 1.0f; float max = 1.0f;
// translate from range [min, max] to range [0, max-min] // translate from range [min, max] to range [0, max-min]
@ -1128,7 +1129,7 @@ static void ProportionPidsSmoothToQuick()
ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise); ComputeStabilizationAndSetPidsFromDampAndNoise(damp, noise);
// save it to the system, but not yet written to flash // save it to the system, but not yet written to flash
SystemIdentSettingsSmoothQuickSettingSet(&smoothQuickSetting); SystemIdentSettingsSmoothQuickValueSet(&smoothQuickValue);
} }

2
shared/uavobjectdefinition/gyrosensor.xml
View File

@ -5,7 +5,7 @@
<field name="y" units="deg/s" type="float" elements="1"/> <field name="y" units="deg/s" type="float" elements="1"/>
<field name="z" units="deg/s" type="float" elements="1"/> <field name="z" units="deg/s" type="float" elements="1"/>
<field name="temperature" units="deg C" type="float" elements="1"/> <field name="temperature" units="deg C" type="float" elements="1"/>
<field name="SensorReadTimestamp" units="PiosRaw" type="uint32" elements="1"/> <field name="SensorReadTimestamp" units="tick" type="uint32" elements="1" description="STM32 CPU clock ticks"/>
<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"/>

2
shared/uavobjectdefinition/gyrostate.xml
View File

@ -4,7 +4,7 @@
<field name="x" units="deg/s" type="float" elements="1"/> <field name="x" units="deg/s" type="float" elements="1"/>
<field name="y" units="deg/s" type="float" elements="1"/> <field name="y" units="deg/s" type="float" elements="1"/>
<field name="z" units="deg/s" type="float" elements="1"/> <field name="z" units="deg/s" type="float" elements="1"/>
<field name="SensorReadTimestamp" units="PiosRaw" type="uint32" elements="1"/> <field name="SensorReadTimestamp" units="tick" type="uint32" elements="1" description="STM32 CPU clock ticks"/>
<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"/>

4
shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -33,7 +33,7 @@
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5"/> <field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5"/>
<field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1"/> <field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1"/>
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="3" limits="%BE:1:3"/> <field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="3" limits="%BE:1:3"/>
<field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60" limits="%BI:0" /> <field name="TuningDuration" units="s" type="uint8" elements="1" defaultvalue="60" limits="%BI:0" />
<!-- SmoothQuickSource: the smooth vs. quick PID selector --> <!-- SmoothQuickSource: the smooth vs. quick PID selector -->
<!-- 0 = disabled --> <!-- 0 = disabled -->
<!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs --> <!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
@ -47,7 +47,7 @@
<!-- 27 (7 stops) means stops at 50, 67, 83, 100, 0, 17, 33 (repeat as you toggle) --> <!-- 27 (7 stops) means stops at 50, 67, 83, 100, 0, 17, 33 (repeat as you toggle) -->
<!-- 25 is special in that the 3 middle values (25, 50, 75) are exactly those that are recommended for smooth, normal, and quick responses --> <!-- 25 is special in that the 3 middle values (25, 50, 75) are exactly those that are recommended for smooth, normal, and quick responses -->
<field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/> <field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/>
<field name="SmoothQuickSetting" units="" type="float" elements="1" defaultvalue="0.0"/> <field name="SmoothQuickValue" units="" type="float" elements="1" defaultvalue="0.0"/>
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/> <field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.001"/> <field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.001"/>
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/> <field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>