From 150dbebc635fb965ef1b01bac99bb55b29b81240 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sun, 15 Dec 2013 22:02:06 -0500 Subject: [PATCH 01/11] OP-1117 MultiWiiHorizon flight mode only (not cliffs H1 or H2) --- flight/modules/ManualControl/manualcontrol.c | 81 ++++++++-------- flight/modules/Stabilization/stabilization.c | 92 ++++++++++++++++--- .../manualcontrolsettings.xml | 6 +- .../stabilizationdesired.xml | 2 +- 4 files changed, 129 insertions(+), 52 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index db1ae3cce..f6dea6930 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -690,46 +690,53 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont return; } + stabilization.Roll = + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : + 0; // this is an invalid mode + + stabilization.Pitch = + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode.Roll = stab_settings[0]; stabilization.StabilizationMode.Pitch = stab_settings[1]; - stabilization.StabilizationMode.Yaw = stab_settings[2]; - - stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode - ; - stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? - cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode - - stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode + // Other axes (yaw) cannot be Horizon, so use Rate + // Should really do this for Attitude mode as well? + if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) { + stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; + } + else { + stabilization.StabilizationMode.Yaw = stab_settings[2]; + stabilization.Yaw = + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + 0; // this is an invalid mode + } stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 3ca9f3afc..23a962937 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -246,9 +246,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) #else /* if defined(PIOS_QUATERNION_STABILIZATION) */ // Simpler algorithm for CC, less memory - float local_error[3] = { stabDesired.Roll - attitudeState.Roll, + float local_error[3] = { stabDesired.Roll - attitudeState.Roll, stabDesired.Pitch - attitudeState.Pitch, - stabDesired.Yaw - attitudeState.Yaw }; + stabDesired.Yaw - attitudeState.Yaw }; // find shortest way float modulo = fmodf(local_error[2] + 180.0f, 360.0f); if (modulo < 0) { @@ -263,7 +263,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyroStateData.y * (1 - gyro_alpha); gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyroStateData.z * (1 - gyro_alpha); - float *attitudeDesiredAxis = &stabDesired.Roll; + float *stabDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; @@ -287,8 +287,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } // Store to rate desired variable for storing to UAVO + // this bound() seems unnecessary rateDesiredAxis[i] = - bound(attitudeDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); + bound(stabDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); @@ -313,10 +314,74 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON: + // A parameterization from Attitude mode at center stick + // - to Rate mode at full stick + // This is done by parameterizing to use the rotation rate that Attitude mode + // - would use at center stick to using the rotation rate that Rate mode + // - would use at full stick in a weighted average sort of way. + { + if (reinit) { + pids[PID_ROLL + i].iAccumulator = 0; + pids[PID_RATE_ROLL + i].iAccumulator = 0; + } + + // Compute what Rate mode would give for this stick angle's rate + // Save in a Rate's rate in a temp for later merging with Attitude's rate + // This bound() seems unnecessary both here and in Rate mode where this came from + float rateDesiredAxisRate; + rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f) + * cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]; + + // Compute what Attitude mode would give for this stick angle's rate + + // stabDesired for this mode is [-1.0f,+1.0f] + // - multiply by Attitude mode max angle to get desired angle + // - subtract off the actual angle to get the angle error + // This is what local_error[] holds for Attitude mode + float attitude_error = stabDesiredAxis[i] + * cast_struct_to_array(settings.RollMax, settings.RollMax)[i] + - cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i]; + + // Compute the outer loop just like Attitude mode does + float rateDesiredAxisAttitude; + rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT); + rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude, + cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]); + + // Compute the weighted average rate desired + // Using max() rather than sqrt() for cpu speed; + // - this makes the stick region into a square; + // - this is a feature! + // - hold a roll angle and add just pitch without the stick sensitivity changing + // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch); + float magnitude; + magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); + rateDesiredAxis[i] = (1.0f-magnitude) * rateDesiredAxisAttitude + magnitude * rateDesiredAxisRate; + + // Compute the inner loop + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); + + // TODO: put a configurable scale factor in for the PID zeroing? + // Do we need a setting for this to dial down the iAccumulator zeroing + // - so both iAccumulators don't get held too close to zero at mid stick? + + // Not sure if this is the best way to do this but I suspect that there + // - would be severe windup without it since they fight each other. + + // At magnitudes close to one, the Attitude accumulator gets zeroed + pids[PID_ROLL + i].iAccumulator *= (1.0f-magnitude); // * factor; + // At magnitudes close to zero, the Rate accumulator gets zeroed + pids[PID_RATE_ROLL + i].iAccumulator *= magnitude; // * factor; + + break; + } + case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: // Store for debugging output - rateDesiredAxis[i] = attitudeDesiredAxis[i]; + rateDesiredAxis[i] = stabDesiredAxis[i]; // Run a virtual flybar stabilization algorithm on this axis stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); @@ -324,6 +389,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: + // FIXME: local_error[] is rate - attitude for Weak Leveling + // The only ramifications are: + // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS + // Changing Rate mode max rate currently requires a change to Kp + // That would be changed to Attitude mode max angle affecting Kp { if (reinit) { pids[PID_RATE_ROLL + i].iAccumulator = 0; @@ -333,7 +403,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) weak_leveling = bound(weak_leveling, weak_leveling_max); // Compute desired rate as input biased towards leveling - rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling; actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); @@ -345,13 +415,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) pids[PID_RATE_ROLL + i].iAccumulator = 0; } - if (fabsf(attitudeDesiredAxis[i]) > max_axislock_rate) { + if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode - rateDesiredAxis[i] = attitudeDesiredAxis[i]; + rateDesiredAxis[i] = stabDesiredAxis[i]; axis_lock_accum[i] = 0; } else { // For weaker commands or no command simply attitude lock (almost) on no gyro change - axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; + axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT; axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } @@ -366,7 +436,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], + rateDesiredAxis[i] = bound(stabDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); // Run the relay controller which also estimates the oscillation parameters @@ -392,7 +462,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) break; case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: - actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i], 1.0f); + actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f); break; default: error = true; diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 535e3fc8d..03a9bab9a 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -22,17 +22,17 @@ diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index bd70011bb..be2f25b2f 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + From 35f6caa3607b957e8e5be3cad1b46cc832e3b513 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Mon, 16 Dec 2013 13:42:24 -0500 Subject: [PATCH 02/11] OP-1117 create a second set of rate PIDs so MWH can run rate and attitude separately --- flight/modules/Stabilization/stabilization.c | 95 ++++++++++++++------ 1 file changed, 67 insertions(+), 28 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 23a962937..8e66c21b3 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -71,8 +71,11 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 4) #define FAILSAFE_TIMEOUT_MS 30 -enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_MAX }; - +// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode +// The PID_RATE set is used by the attitude portion of Attitude mode +// The PID_RATEA_ROLL set is used by MultiWiiHorizon mode because it needs to maintain +// - two independant rate PIDs because it does rate and attitude simultaneously +enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX }; // Private variables static xTaskHandle taskHandle; @@ -322,8 +325,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // - would use at full stick in a weighted average sort of way. { if (reinit) { - pids[PID_ROLL + i].iAccumulator = 0; - pids[PID_RATE_ROLL + i].iAccumulator = 0; + pids[PID_ROLL + i].iAccumulator = 0; + pids[PID_RATE_ROLL + i].iAccumulator = 0; + pids[PID_RATEA_ROLL + i].iAccumulator = 0; } // Compute what Rate mode would give for this stick angle's rate @@ -359,8 +363,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); rateDesiredAxis[i] = (1.0f-magnitude) * rateDesiredAxisAttitude + magnitude * rateDesiredAxisRate; - // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); + // Compute the inner loop for both Rate mode and Attitude mode + // actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates + actuatorDesiredAxis[i] = + (1.0f-magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT) + + magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); // TODO: put a configurable scale factor in for the PID zeroing? @@ -370,10 +377,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Not sure if this is the best way to do this but I suspect that there // - would be severe windup without it since they fight each other. - // At magnitudes close to one, the Attitude accumulator gets zeroed - pids[PID_ROLL + i].iAccumulator *= (1.0f-magnitude); // * factor; - // At magnitudes close to zero, the Rate accumulator gets zeroed - pids[PID_RATE_ROLL + i].iAccumulator *= magnitude; // * factor; + // At magnitudes close to one, the Attitude accumulators gets zeroed + pids[PID_ROLL + i].iAccumulator *= (1.0f-magnitude); // * factor; + pids[PID_RATEA_ROLL + i].iAccumulator *= (1.0f-magnitude); // * factor; + + // At magnitudes close to zero, the Rate accumulator gets zeroed + pids[PID_RATE_ROLL + i].iAccumulator *= magnitude; // * factor; break; } @@ -559,38 +568,68 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) StabilizationSettingsGet(&settings); // Set the roll rate PID constants - pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp, + pid_configure(&pids[PID_RATE_ROLL], + settings.RollRatePID.Kp, settings.RollRatePID.Ki, - pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd, - pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit); + settings.RollRatePID.Kd, + settings.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp, - pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki, - pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd, - pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit); + pid_configure(&pids[PID_RATE_PITCH], + settings.PitchRatePID.Kp, + settings.PitchRatePID.Ki, + settings.PitchRatePID.Kd, + settings.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp, - pids[PID_RATE_YAW].i = settings.YawRatePID.Ki, - pids[PID_RATE_YAW].d = settings.YawRatePID.Kd, - pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit); + pid_configure(&pids[PID_RATE_YAW], + settings.YawRatePID.Kp, + settings.YawRatePID.Ki, + settings.YawRatePID.Kd, + settings.YawRatePID.ILimit); // Set the roll attitude PI constants - pid_configure(&pids[PID_ROLL], settings.RollPI.Kp, - settings.RollPI.Ki, 0, - pids[PID_ROLL].iLim = settings.RollPI.ILimit); + pid_configure(&pids[PID_ROLL], + settings.RollPI.Kp, + settings.RollPI.Ki, + 0, + settings.RollPI.ILimit); // Set the pitch attitude PI constants - pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp, - pids[PID_PITCH].i = settings.PitchPI.Ki, 0, + pid_configure(&pids[PID_PITCH], + settings.PitchPI.Kp, + settings.PitchPI.Ki, + 0, settings.PitchPI.ILimit); // Set the yaw attitude PI constants - pid_configure(&pids[PID_YAW], settings.YawPI.Kp, - settings.YawPI.Ki, 0, + pid_configure(&pids[PID_YAW], + settings.YawPI.Kp, + settings.YawPI.Ki, + 0, settings.YawPI.ILimit); + // Set the MWHorizon roll rate PID constants + pid_configure(&pids[PID_RATEA_ROLL], + settings.RollRatePID.Kp, + settings.RollRatePID.Ki, + settings.RollRatePID.Kd, + settings.RollRatePID.ILimit); + + // Set the MWHorizon pitch rate PID constants + pid_configure(&pids[PID_RATEA_PITCH], + settings.PitchRatePID.Kp, + settings.PitchRatePID.Ki, + settings.PitchRatePID.Kd, + settings.PitchRatePID.ILimit); + + // Set the MWHorizon yaw rate PID constants + pid_configure(&pids[PID_RATEA_YAW], + settings.YawRatePID.Kp, + settings.YawRatePID.Ki, + settings.YawRatePID.Kd, + settings.YawRatePID.ILimit); + // Set up the derivative term pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); From 56b4d8647f66b6daf418b6ce936e889973a35dc9 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 18 Dec 2013 05:01:16 -0500 Subject: [PATCH 03/11] OP-1117 Rename Horizon to Rattitude, Anti Windup and dT additions --- flight/modules/ManualControl/manualcontrol.c | 58 ++--- flight/modules/Stabilization/stabilization.c | 82 +++++-- .../src/plugins/config/stabilization.ui | 212 +++++++++++++++++- .../manualcontrolsettings.xml | 6 +- .../stabilizationdesired.xml | 2 +- .../stabilizationsettings.xml | 2 + 6 files changed, 301 insertions(+), 61 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index f6dea6930..23a3d8bab 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -691,50 +691,50 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont } stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode stabilization.Pitch = - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode.Roll = stab_settings[0]; stabilization.StabilizationMode.Pitch = stab_settings[1]; - // Other axes (yaw) cannot be Horizon, so use Rate + // Other axes (yaw) cannot be Rattitude, so use Rate // Should really do this for Attitude mode as well? - if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) { + if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; } else { stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode } diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 8e66c21b3..fb8d4eb91 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -73,7 +73,7 @@ // The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode // The PID_RATE set is used by the attitude portion of Attitude mode -// The PID_RATEA_ROLL set is used by MultiWiiHorizon mode because it needs to maintain +// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain // - two independant rate PIDs because it does rate and attitude simultaneously enum { PID_RATE_ROLL, PID_RATE_PITCH, PID_RATE_YAW, PID_ROLL, PID_PITCH, PID_YAW, PID_RATEA_ROLL, PID_RATEA_PITCH, PID_RATEA_YAW, PID_MAX }; @@ -91,6 +91,7 @@ bool lowThrottleZeroIntegral; bool lowThrottleZeroAxis[MAX_AXES]; float vbar_decay = 0.991f; struct pid pids[PID_MAX]; +float rattitude_anti_windup; // Private functions static void stabilizationTask(void *parameters); @@ -178,7 +179,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); #endif - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe + // Wait until the AttitudeRaw (wrong, it is Gyro) object is updated, if a timeout then go to failsafe if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); continue; @@ -317,7 +318,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) break; - case STABILIZATIONDESIRED_STABILIZATIONMODE_MULTIWIIHORIZON: + case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: // A parameterization from Attitude mode at center stick // - to Rate mode at full stick // This is done by parameterizing to use the rotation rate that Attitude mode @@ -351,7 +352,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) float rateDesiredAxisAttitude; rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT); rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude, - cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]); + cast_struct_to_array(settings.MaximumRate, + settings.MaximumRate.Roll)[i]); // Compute the weighted average rate desired // Using max() rather than sqrt() for cpu speed; @@ -361,7 +363,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch); float magnitude; magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); - rateDesiredAxis[i] = (1.0f-magnitude) * rateDesiredAxisAttitude + magnitude * rateDesiredAxisRate; + rateDesiredAxis[i] = (1.0f-magnitude) * rateDesiredAxisAttitude + + magnitude * rateDesiredAxisRate; // Compute the inner loop for both Rate mode and Attitude mode // actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates @@ -370,19 +373,58 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) + magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); - // TODO: put a configurable scale factor in for the PID zeroing? - // Do we need a setting for this to dial down the iAccumulator zeroing - // - so both iAccumulators don't get held too close to zero at mid stick? + // settings.RattitudeAntiWindup controls the iAccumulator zeroing + // - so both iAccs don't wind up too far; + // - nor do both iAccs get held too close to zero at mid stick - // Not sure if this is the best way to do this but I suspect that there - // - would be severe windup without it since they fight each other. + // Not sure if this is the best way to do this but I suspect that there would + // - be windup without it since rate and attitude fight each other here + // - rate trying to pull it over the top and attitude trying to pull it back down - // At magnitudes close to one, the Attitude accumulators gets zeroed - pids[PID_ROLL + i].iAccumulator *= (1.0f-magnitude); // * factor; - pids[PID_RATEA_ROLL + i].iAccumulator *= (1.0f-magnitude); // * factor; + // Wind-up increases linearly with cycles for a fixed error. + // We need to cut it down faster than that to keep ahead of it. + // We must never increase the iAcc or we risk oscillation. - // At magnitudes close to zero, the Rate accumulator gets zeroed - pids[PID_RATE_ROLL + i].iAccumulator *= magnitude; // * factor; + // Use the powf() function to make two anti-windup curves + // - one for zeroing rate close to center stick + // - the other for zeroing attitude close to max stick + + // the bigger the dT the more anti windup needed + // the bigger the PID[].i the more anti windup needed + // more anti windup is achieved with a lower powf() power + + // some quick napkin calculations say that 1/10th second, 50 cycles + // to reduce an iAcc by half we should have a factor of about .986 + // this is so that at half stick, iAcc gets reduced to half in .1 second + // this sounds about right for a default anti windup + // so powf(.5, x) = .014 + // .5^x = .014 + // x about 6 + // for rate 6 = 1 / (aw * .002 * .003), aw = 1 / (6 * .002 * .003) = 27777 + // for attitude 6 = 1 / (aw * .002 * 1) , aw = 1 / (6 * .002 * 2.5) = 33 + // multiply by 833 for rate, use as is for attitude + // hand testing showed that aw=10 reduced the windup by maybe half + + // This may only be useful for aircraft with large Ki values and limits + if (dT > 0.0f && rattitude_anti_windup > 0.0f) { + float factor; + + // At magnitudes close to one, the Attitude accumulators gets zeroed + if (pids[PID_ROLL+i].i > 0.0f) { + factor = 1.0f - powf(magnitude, 1.0f / (rattitude_anti_windup * dT * pids[PID_ROLL+i].i)); + pids[PID_ROLL+i].iAccumulator *= factor; + } + if (pids[PID_RATEA_ROLL+i].i > 0.0f) { + factor = 1.0f - powf(magnitude, 1.0f / (rattitude_anti_windup * dT * pids[PID_RATEA_ROLL+i].i * 833.0f)); + pids[PID_RATEA_ROLL+i].iAccumulator *= factor; + } + + // At magnitudes close to zero, the Rate accumulator gets zeroed + if (pids[PID_RATE_ROLL+i].i > 0.0f) { + factor = 1.0f - powf(1.0f-magnitude, 1.0f / (rattitude_anti_windup * dT * pids[PID_RATE_ROLL+i].i * 833.0f)); + pids[PID_RATE_ROLL+i].iAccumulator *= factor; + } + } break; } @@ -403,6 +445,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS // Changing Rate mode max rate currently requires a change to Kp // That would be changed to Attitude mode max angle affecting Kp + // Also does not take dT into account { if (reinit) { pids[PID_RATE_ROLL + i].iAccumulator = 0; @@ -609,21 +652,21 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) 0, settings.YawPI.ILimit); - // Set the MWHorizon roll rate PID constants + // Set the Rattitude roll rate PID constants pid_configure(&pids[PID_RATEA_ROLL], settings.RollRatePID.Kp, settings.RollRatePID.Ki, settings.RollRatePID.Kd, settings.RollRatePID.ILimit); - // Set the MWHorizon pitch rate PID constants + // Set the Rattitude pitch rate PID constants pid_configure(&pids[PID_RATEA_PITCH], settings.PitchRatePID.Kp, settings.PitchRatePID.Ki, settings.PitchRatePID.Kd, settings.PitchRatePID.ILimit); - // Set the MWHorizon yaw rate PID constants + // Set the Rattitude yaw rate PID constants pid_configure(&pids[PID_RATEA_YAW], settings.YawRatePID.Kp, settings.YawRatePID.Ki, @@ -663,6 +706,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) // Compute time constant for vbar decay term based on a tau vbar_decay = expf(-fakeDt / settings.VbarTau); + + // Rattitude flight mode anti-windup factor + rattitude_anti_windup = (float) settings.RattitudeAntiWindup; } diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 84291366e..ac3204aad 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -507,7 +507,7 @@ QTabWidget::Rounded - 0 + 2 false @@ -627,8 +627,8 @@ 0 0 - 778 - 659 + 798 + 656 @@ -8867,9 +8867,9 @@ border-radius: 5; 0 - -117 - 778 - 762 + 0 + 784 + 711 @@ -18908,8 +18908,8 @@ border-radius: 5; 0 0 - 792 - 645 + 784 + 673 @@ -21554,6 +21554,198 @@ border-radius: 5; + + + + Rattitude + + + + 9 + + + 9 + + + 9 + + + 9 + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Default + + + + objname:StabilizationSettings + button:default + buttongroup:15 + + + + + + + + QGroupBox{border: 0px;} + + + true + + + + 0 + + + 9 + + + 0 + + + 0 + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 90 + 11 + + + + + + + + + 0 + 0 + + + + + 5 + 22 + + + + + 175 + 22 + + + + Qt::StrongFocus + + + <html><head/><body><p>Higher values will keep larger Ki terms and limits from winding up at partial stick. Consider increasing this if you have high Ki values and limits and a sudden stick motion from one aircraft bank angle to another causes the aircraft to rotate and then slowly change rotation.</p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 + + + 0.000000000000000 + + + 255.000000000000000 + + + 10.000000000000000 + + + + objname:StabilizationSettings + fieldname:RattitudeAntiWindup + haslimits:no + scale:1 + buttongroup:15 + + + + + + + + + 0 + 0 + + + + + 144 + 16 + + + + + 175 + 16777215 + + + + + 75 + true + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; + + + Anti-Windup + + + Qt::AlignCenter + + + + + + + Qt::Horizontal + + + + 300 + 20 + + + + + + + + + + @@ -27021,8 +27213,8 @@ border-radius: 5; 0 0 - 792 - 645 + 798 + 656 diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 03a9bab9a..38bea2b4a 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -22,17 +22,17 @@ diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index be2f25b2f..a93e1f34c 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 6979fb9f4..ca26df113 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -33,6 +33,8 @@ + + From 13f500a67d037f41164501220a50db31c4e51a12 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Wed, 18 Dec 2013 14:50:44 -0500 Subject: [PATCH 04/11] OP-1117 move default tab back to the beginning --- ground/openpilotgcs/src/plugins/config/stabilization.ui | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index ac3204aad..8a4ccb1fd 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -507,7 +507,7 @@ QTabWidget::Rounded - 2 + 0 false @@ -8868,7 +8868,7 @@ border-radius: 5; 0 0 - 784 + 545 711 @@ -27213,8 +27213,8 @@ border-radius: 5; 0 0 - 798 - 656 + 407 + 518 From 14aa1f83de573ecebe096b063dfa8bd313ed4474 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 19 Dec 2013 03:21:30 -0500 Subject: [PATCH 05/11] OP-1117 make anti windup insensitive to Ki changes, include dT in calcs, linearize config spinner --- flight/modules/Stabilization/stabilization.c | 75 ++++++++++++++++--- .../src/plugins/config/stabilization.ui | 2 +- .../stabilizationsettings.xml | 2 +- 3 files changed, 65 insertions(+), 14 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index fb8d4eb91..f199d70d0 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -99,6 +99,10 @@ static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent *ev); +// temp log2f() because of +// error: unsuffixed float constant +static float stab_log2f(float x); + /** * Module initialization */ @@ -179,7 +183,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); #endif - // Wait until the AttitudeRaw (wrong, it is Gyro) object is updated, if a timeout then go to failsafe + // Wait until the Gyro object is updated, if a timeout then go to failsafe if (xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING); continue; @@ -291,7 +295,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } // Store to rate desired variable for storing to UAVO - // this bound() seems unnecessary rateDesiredAxis[i] = bound(stabDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); @@ -333,7 +336,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute what Rate mode would give for this stick angle's rate // Save in a Rate's rate in a temp for later merging with Attitude's rate - // This bound() seems unnecessary both here and in Rate mode where this came from float rateDesiredAxisRate; rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f) * cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]; @@ -377,12 +379,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // - so both iAccs don't wind up too far; // - nor do both iAccs get held too close to zero at mid stick - // Not sure if this is the best way to do this but I suspect that there would - // - be windup without it since rate and attitude fight each other here + // I suspect that there would be windup without it + // - since rate and attitude fight each other here // - rate trying to pull it over the top and attitude trying to pull it back down // Wind-up increases linearly with cycles for a fixed error. - // We need to cut it down faster than that to keep ahead of it. // We must never increase the iAcc or we risk oscillation. // Use the powf() function to make two anti-windup curves @@ -392,18 +393,54 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // the bigger the dT the more anti windup needed // the bigger the PID[].i the more anti windup needed // more anti windup is achieved with a lower powf() power + // a doubling of e.g. PID[].i should cause the anti windup factor + // to get twice as far away from 1.0 (towards zero) + // e.g. from .90 to .80 // some quick napkin calculations say that 1/10th second, 50 cycles // to reduce an iAcc by half we should have a factor of about .986 // this is so that at half stick, iAcc gets reduced to half in .1 second - // this sounds about right for a default anti windup + // this sounds in the ballpark for a default anti windup // so powf(.5, x) = .014 // .5^x = .014 // x about 6 // for rate 6 = 1 / (aw * .002 * .003), aw = 1 / (6 * .002 * .003) = 27777 // for attitude 6 = 1 / (aw * .002 * 1) , aw = 1 / (6 * .002 * 2.5) = 33 - // multiply by 833 for rate, use as is for attitude - // hand testing showed that aw=10 reduced the windup by maybe half + // multiply by 833 for rate, use aw as is for attitude + // hand testing showed that aw=10 reduced the windup slightly + + // make Ki a multiplicative factor, not a power factor + // given magnitude=.5, to jump from a factor of .75 (pow()=.25) + // to a factor of .92 (~3x) (pow()=.08) + // we need to multiply by 1/3 (factor of 3) + // log(a+b) = log(a) * log(b) + // powf(.5, x) = .33, .5^x=.333, x=log.5(.333)=-log2(.333) + + // logb(x) = loga(x)/loga(b) + + // assume a loop rate of 625 iterations per second + // I see about 15x 1.5ms updates and then a 3.0ms update on the average + // assuming dT averages about 0.0016 + + // magic numbers + // 255 comes from uint_8 scaling + // 37.8387 is so that if the uavo is 100, the power is 23 + // these calculations are for magnitude = 0.5 so 23 corresponds to the number of bits + // used in the mantissa of the float + // i.e. 1.0-(0.5^23) almost underflows + // the 17.668f and 7.966f cancel the default value of the log2() that follow them + + // these generate the inverted parabola like curves that go through [0,1] and [1,0] + // powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - 17.668f - log2f(dT * pids[PID_RATE_ROLL+i].i)); + // powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - 7.966f - log2f(dT * pids[PID_ROLL+i].i)); + // for uavo 255 the power is 0 the factor is constant 0 and anti windup erases all of iAcc + // for uavo 248 the power is 1 (approx) the curve is a line + // for uavo 242 the power is 2 (approx) the curve is a parabola + // for uavo 235 the power is 3 (approx) the curve is a cubic + // for higher powers the curve becomes more like a pair of straight lines + // for uavo 100 the power is 23 + // for uavo 1 the power is 37.7 + // for uavo 0 disable anti windup // This may only be useful for aircraft with large Ki values and limits if (dT > 0.0f && rattitude_anti_windup > 0.0f) { @@ -411,17 +448,20 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // At magnitudes close to one, the Attitude accumulators gets zeroed if (pids[PID_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(magnitude, 1.0f / (rattitude_anti_windup * dT * pids[PID_ROLL+i].i)); + factor = 1.0f - powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup + - 7.966f - stab_log2f(dT * pids[PID_ROLL+i].i)); pids[PID_ROLL+i].iAccumulator *= factor; } if (pids[PID_RATEA_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(magnitude, 1.0f / (rattitude_anti_windup * dT * pids[PID_RATEA_ROLL+i].i * 833.0f)); + factor = 1.0f - powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup + - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL+i].i)); pids[PID_RATEA_ROLL+i].iAccumulator *= factor; } // At magnitudes close to zero, the Rate accumulator gets zeroed if (pids[PID_RATE_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(1.0f-magnitude, 1.0f / (rattitude_anti_windup * dT * pids[PID_RATE_ROLL+i].i * 833.0f)); + factor = 1.0f - powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup + - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL+i].i)); pids[PID_RATE_ROLL+i].iAccumulator *= factor; } } @@ -606,6 +646,17 @@ static float bound(float val, float range) return val; } +// because of +// error: unsuffixed float constant +static float stab_log2f(float x) +{ + static float factor = 0.0f; + if (factor <= 0.0f) { + factor = logf(2.0f); + } + return (logf(x) / factor); +} + static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { StabilizationSettingsGet(&settings); diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 8a4ccb1fd..4a0b12105 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -21675,7 +21675,7 @@ border-radius: 5; 255.000000000000000 - 10.000000000000000 + 100.000000000000000 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index ca26df113..ed522d230 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -33,7 +33,7 @@ - + From 54ac7da2e8ea44438a40259721193c8a330f2218 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Thu, 2 Jan 2014 00:56:55 -0500 Subject: [PATCH 06/11] OP-1117 bugfix for refactoring, would keep Ki from working at full stick --- flight/modules/Stabilization/stabilization.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index f199d70d0..2f80d50f1 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -460,7 +460,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // At magnitudes close to zero, the Rate accumulator gets zeroed if (pids[PID_RATE_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup + factor = 1.0f - powf(1.0f-magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL+i].i)); pids[PID_RATE_ROLL+i].iAccumulator *= factor; } From d429f3ec814d77189d00b729f4ac13d22a1a7f81 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 3 Jan 2014 14:58:01 +0100 Subject: [PATCH 07/11] Add Eclipse .settings and 'core' files to git ignore list --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 1a7980a39..0deef206c 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,7 @@ GPATH GRTAGS GSYMS GTAGS +core # flight /flight/*.pnproj @@ -56,6 +57,7 @@ openpilotgcs-build-desktop /.cproject /.project /.metadata +/.settings # Ignore Eclipse temp folder, git plugin based? RemoteSystemsTempFiles From a48b9cc24274599aee1db2b245e57f11b05bd49d Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Sat, 4 Jan 2014 04:22:54 -0500 Subject: [PATCH 08/11] OP-1117 reduced CPU for CC/3D coded log2f() / powf() --- flight/modules/Stabilization/stabilization.c | 119 +++++++++--------- flight/uavobjects/eventdispatcher.c | 1 + .../src/plugins/config/stabilization.ui | 4 +- .../stabilizationsettings.xml | 2 +- 4 files changed, 61 insertions(+), 65 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 2f80d50f1..21218c394 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -91,17 +91,15 @@ bool lowThrottleZeroIntegral; bool lowThrottleZeroAxis[MAX_AXES]; float vbar_decay = 0.991f; struct pid pids[PID_MAX]; -float rattitude_anti_windup; +static uint8_t rattitude_anti_windup; // Private functions static void stabilizationTask(void *parameters); static float bound(float val, float range); static void ZeroPids(void); static void SettingsUpdatedCb(UAVObjEvent *ev); - -// temp log2f() because of -// error: unsuffixed float constant static float stab_log2f(float x); +static float stab_powf(float x, uint8_t p); /** * Module initialization @@ -335,7 +333,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } // Compute what Rate mode would give for this stick angle's rate - // Save in a Rate's rate in a temp for later merging with Attitude's rate + // Save Rate's rate in a temp for later merging with Attitude's rate float rateDesiredAxisRate; rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f) * cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]; @@ -393,54 +391,26 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // the bigger the dT the more anti windup needed // the bigger the PID[].i the more anti windup needed // more anti windup is achieved with a lower powf() power - // a doubling of e.g. PID[].i should cause the anti windup factor + // a doubling of e.g. PID[].i should cause the runtime anti windup factor // to get twice as far away from 1.0 (towards zero) // e.g. from .90 to .80 - // some quick napkin calculations say that 1/10th second, 50 cycles - // to reduce an iAcc by half we should have a factor of about .986 - // this is so that at half stick, iAcc gets reduced to half in .1 second - // this sounds in the ballpark for a default anti windup - // so powf(.5, x) = .014 - // .5^x = .014 - // x about 6 - // for rate 6 = 1 / (aw * .002 * .003), aw = 1 / (6 * .002 * .003) = 27777 - // for attitude 6 = 1 / (aw * .002 * 1) , aw = 1 / (6 * .002 * 2.5) = 33 - // multiply by 833 for rate, use aw as is for attitude - // hand testing showed that aw=10 reduced the windup slightly - - // make Ki a multiplicative factor, not a power factor - // given magnitude=.5, to jump from a factor of .75 (pow()=.25) - // to a factor of .92 (~3x) (pow()=.08) - // we need to multiply by 1/3 (factor of 3) - // log(a+b) = log(a) * log(b) - // powf(.5, x) = .33, .5^x=.333, x=log.5(.333)=-log2(.333) - - // logb(x) = loga(x)/loga(b) - - // assume a loop rate of 625 iterations per second - // I see about 15x 1.5ms updates and then a 3.0ms update on the average - // assuming dT averages about 0.0016 - // magic numbers - // 255 comes from uint_8 scaling - // 37.8387 is so that if the uavo is 100, the power is 23 - // these calculations are for magnitude = 0.5 so 23 corresponds to the number of bits - // used in the mantissa of the float - // i.e. 1.0-(0.5^23) almost underflows - // the 17.668f and 7.966f cancel the default value of the log2() that follow them - // these generate the inverted parabola like curves that go through [0,1] and [1,0] - // powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - 17.668f - log2f(dT * pids[PID_RATE_ROLL+i].i)); - // powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - 7.966f - log2f(dT * pids[PID_ROLL+i].i)); - // for uavo 255 the power is 0 the factor is constant 0 and anti windup erases all of iAcc - // for uavo 248 the power is 1 (approx) the curve is a line - // for uavo 242 the power is 2 (approx) the curve is a parabola - // for uavo 235 the power is 3 (approx) the curve is a cubic - // for higher powers the curve becomes more like a pair of straight lines - // for uavo 100 the power is 23 - // for uavo 1 the power is 37.7 - // for uavo 0 disable anti windup + // the higher the power, the closer the curve is to a straight line + // from [0,1] to [1,1] to [1,0] and the less anti windup is applied + + // the UAVO RattitudeAntiWindup varies from 0 to 31 + // 0 turns off anti windup + // 1 gives very little anti-windup because the power given to powf() is 31 + // 31 gives a lot of anti-windup because the power given to powf() is 1 + // the 32.1 is what does this + // the 7.966 and 17.668 cancel the default PID value and dT given to log2f() + // if these are non-default, tweaking is thus done so the user doesn't have to readjust + // the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22 + // these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits + // used in the mantissa of the float + // i.e. 1.0-(0.5^22) almost underflows // This may only be useful for aircraft with large Ki values and limits if (dT > 0.0f && rattitude_anti_windup > 0.0f) { @@ -448,20 +418,17 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // At magnitudes close to one, the Attitude accumulators gets zeroed if (pids[PID_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - - 7.966f - stab_log2f(dT * pids[PID_ROLL+i].i)); + factor = 1.0f - stab_powf(magnitude, ((uint8_t) (32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL+i].i))) - rattitude_anti_windup); pids[PID_ROLL+i].iAccumulator *= factor; } if (pids[PID_RATEA_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL+i].i)); + factor = 1.0f - stab_powf(magnitude, ((uint8_t) (32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL+i].i))) - rattitude_anti_windup); pids[PID_RATEA_ROLL+i].iAccumulator *= factor; } // At magnitudes close to zero, the Rate accumulator gets zeroed if (pids[PID_RATE_ROLL+i].i > 0.0f) { - factor = 1.0f - powf(1.0f-magnitude, 37.8387f - (37.8387f/255.0f) * rattitude_anti_windup - - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL+i].i)); + factor = 1.0f - stab_powf(1.0f-magnitude, ((uint8_t) (32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL+i].i))) - rattitude_anti_windup); pids[PID_RATE_ROLL+i].iAccumulator *= factor; } } @@ -646,17 +613,45 @@ static float bound(float val, float range) return val; } -// because of -// error: unsuffixed float constant + +// x small (0.0 < x < .01) so interpolation of fractional part is reasonable static float stab_log2f(float x) { - static float factor = 0.0f; - if (factor <= 0.0f) { - factor = logf(2.0f); - } - return (logf(x) / factor); + union + { + volatile float f; + volatile uint32_t i; + volatile unsigned char c[4]; + } __attribute__((packed)) u1, u2; + + u2.f = u1.f = x; + u1.i <<= 1; + u2.i &= 0xff800000; + + // get and unbias the exponent, add in a linear interpolation of the fractional part + return (float) (u1.c[3] - 127) + (x / u2.f) - 1.0f; } + +// 0<=x<=1, 0<=p<=31 +static float stab_powf(float x, uint8_t p) +{ + float retval = 1.0f; + + while (p) + { + if (p&1) + { + retval *= x; + } + x *= x; + p >>= 1; + } + + return retval; +} + + static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { StabilizationSettingsGet(&settings); @@ -759,7 +754,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) vbar_decay = expf(-fakeDt / settings.VbarTau); // Rattitude flight mode anti-windup factor - rattitude_anti_windup = (float) settings.RattitudeAntiWindup; + rattitude_anti_windup = settings.RattitudeAntiWindup; } diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 9fc78a21c..d9518d620 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -104,6 +104,7 @@ int32_t EventDispatcherInitialize() // Create callback eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4); + DelayedCallbackDispatch(eventSchedulerCallback); // Done return 0; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 4a0b12105..61352bd2f 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -21672,10 +21672,10 @@ border-radius: 5; 0.000000000000000 - 255.000000000000000 + 31.000000000000000 - 100.000000000000000 + 10.000000000000000 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index ed522d230..ca26df113 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -33,7 +33,7 @@ - + From 6b3a02c485cf200624d04dbc6ce5ada330c42f9a Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 5 Jan 2014 18:45:57 +0100 Subject: [PATCH 09/11] [OP-1105] improved handling of missing firmware description in GCS --- .../plugins/config/configpipxtremewidget.cpp | 4 +- .../uavobjectutil/uavobjectutilmanager.cpp | 9 ++++- .../uavobjectutil/uavobjectutilmanager.h | 1 + .../src/plugins/uploader/devicewidget.cpp | 37 ++++++++----------- .../src/plugins/uploader/op_dfu.cpp | 6 ++- .../plugins/uploader/runningdevicewidget.cpp | 24 ++++++------ 6 files changed, 41 insertions(+), 40 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index b9035e7fe..d8a495a82 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -168,8 +168,9 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) } // Update the Description field + // TODO use UAVObjectUtilManager::descriptionToStructure() UAVObjectField *descField = object->getField("Description"); - if (descField) { + if (descField && descField->getValue(0) != QChar(255)) { /* * This looks like a binary with a description at the end: * 4 bytes: header: "OpFw". @@ -196,6 +197,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) m_oplink->FirmwareVersion->setText(descstr + " " + date); } else { qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + m_oplink->FirmwareVersion->setText(tr("Unknown")); } // Update the serial number field diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index a8acfc752..a9b5afaa7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -301,6 +301,13 @@ QByteArray UAVObjectUtilManager::getBoardDescription() return ret; } +QString UAVObjectUtilManager::getBoardDescriptionString() +{ + QByteArray arr = getBoardDescription(); + + int index = arr.indexOf(255); + return QString((index == -1) ? arr : arr.left(index)); +} // ****************************** // HomeLocation @@ -474,5 +481,3 @@ bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescrip } return false; } - -// ****************************** diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h index 7ad27b8d1..a89e241f7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.h @@ -61,6 +61,7 @@ public: QByteArray getBoardCPUSerial(); quint32 getFirmwareCRC(); QByteArray getBoardDescription(); + QString getBoardDescriptionString(); deviceDescriptorStruct getBoardDescriptionStruct(); static bool descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc); UAVObjectManager *getObjectManager(); diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index 0cb91125a..9afbe5bdb 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -41,8 +41,7 @@ DeviceWidget::DeviceWidget(QWidget *parent) : connect(myDevice->updateButton, SIGNAL(clicked()), this, SLOT(uploadFirmware())); connect(myDevice->pbLoad, SIGNAL(clicked()), this, SLOT(loadFirmware())); connect(myDevice->confirmCheckBox, SIGNAL(stateChanged(int)), this, SLOT(confirmCB(int))); - QPixmap pix = QPixmap(QString(":uploader/images/view-refresh.svg")); - myDevice->statusIcon->setPixmap(pix); + myDevice->statusIcon->setPixmap(QPixmap(":uploader/images/view-refresh.svg")); myDevice->lblCertified->setText(""); } @@ -79,12 +78,12 @@ void DeviceWidget::populate() { int id = m_dfu->devices[deviceID].ID; - myDevice->lbldevID->setText(QString("Device ID: ") + QString::number(id, 16)); + myDevice->lbldevID->setText(tr("Device ID: ") + QString::number(id, 16)); // DeviceID tells us what sort of HW we have detected: // display a nice icon: myDevice->gVDevice->scene()->clear(); myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id)); - myDevice->lblHWRev->setText(QString(tr("HW Revision: ")) + QString::number(id & 0x00FF, 16)); + myDevice->lblHWRev->setText(tr("HW Revision: ") + QString::number(id & 0x00FF, 16)); switch (id) { case 0x0101: @@ -115,25 +114,23 @@ void DeviceWidget::populate() bool r = m_dfu->devices[deviceID].Readable; bool w = m_dfu->devices[deviceID].Writable; - myDevice->lblAccess->setText(QString("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); - myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); + myDevice->lblAccess->setText(tr("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-")); + myDevice->lblMaxCode->setText(tr("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode)); myDevice->lblCRC->setText(QString::number(m_dfu->devices[deviceID].FW_CRC)); - myDevice->lblBLVer->setText(QString("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); + myDevice->lblBLVer->setText(tr("BL version: ") + QString::number(m_dfu->devices[deviceID].BL_Version)); int size = ((OP_DFU::device)m_dfu->devices[deviceID]).SizeOfDesc; m_dfu->enterDFU(deviceID); QByteArray desc = m_dfu->DownloadDescriptionAsBA(size); if (!populateBoardStructuredDescription(desc)) { - // TODO // desc was not a structured description QString str = m_dfu->DownloadDescription(size); - myDevice->lblDescription->setText(QString("Firmware custom description: ") + str.left(str.indexOf(QChar(255)))); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblDescription->setText((!str.isEmpty()) ? str : tr("Unknown")); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); - myDevice->lblBuildDate->setText("Warning: development firmware"); - myDevice->lblGitTag->setText(""); - myDevice->lblBrdName->setText(""); + myDevice->lblBuildDate->setText(tr("Unknown")); + myDevice->lblGitTag->setText(tr("Unknown")); + myDevice->lblBrdName->setText(tr("Unknown")); } status("Ready...", STATUSICON_INFO); @@ -179,13 +176,11 @@ bool DeviceWidget::populateBoardStructuredDescription(QByteArray desc) myDevice->lblBuildDate->setText(onBoardDescription.gitDate.insert(4, "-").insert(7, "-")); if (onBoardDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescription->setText(onBoardDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); } else { myDevice->lblDescription->setText(onBoardDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } @@ -205,14 +200,12 @@ bool DeviceWidget::populateLoadedStructuredDescription(QByteArray desc) if (LoadedDescription.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); - myDevice->lblCertifiedL->setPixmap(pix); + myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/application-certificate.svg")); myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build")); } else { myDevice->lblDescritpionL->setText(LoadedDescription.gitTag); myDevice->description->setText(LoadedDescription.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertifiedL->setPixmap(pix); + myDevice->lblCertifiedL->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision)); diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 2ebb1495e..c3dcd20ad 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -342,8 +342,9 @@ QString DFUObject::DownloadDescription(int const & numberOfChars) QByteArray arr; StartDownloadT(&arr, numberOfChars, OP_DFU::Descript); - QString str(arr); - return str; + + int index = arr.indexOf(255); + return QString((index == -1) ? arr : arr.left(index)); } QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars) @@ -351,6 +352,7 @@ QByteArray DFUObject::DownloadDescriptionAsBA(int const & numberOfChars) QByteArray arr; StartDownloadT(&arr, numberOfChars, OP_DFU::Descript); + return arr; } diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index 904080ef5..b433f9641 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -104,24 +104,22 @@ void RunningDeviceWidget::populate() deviceDescriptorStruct devDesc; if (UAVObjectUtilManager::descriptionToStructure(description, devDesc)) { if (devDesc.gitTag.startsWith("RELEASE", Qt::CaseSensitive)) { - myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); } else { - myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } - myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash); - myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); + myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + devDesc.gitHash); + myDevice->lblFWDate->setText(tr("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-")); } else { - myDevice->lblFWTag->setText(QString("Firmware tag: ") + QString(description).left(QString(description).indexOf(QChar(255)))); - myDevice->lblGitCommitTag->setText("Git commit tag: Unknown"); - myDevice->lblFWDate->setText(QString("Firmware date: Unknown")); - QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); - myDevice->lblCertified->setPixmap(pix); + QString desc = utilMngr->getBoardDescriptionString(); + myDevice->lblFWTag->setText(tr("Firmware tag: ") + (!desc.isEmpty() ? desc : tr("Unknown"))); + myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg")); myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); + myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + tr("Unknown")); + myDevice->lblFWDate->setText(tr("Firmware date: ") + tr("Unknown")); } } From 329ea821f8e984124f6616d0b14a22fc101dea97 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 5 Jan 2014 22:27:54 +0100 Subject: [PATCH 10/11] [OP-1105] improved handling of missing firmware description in GCS - fixed logging spam introduced by previous commit --- .../plugins/config/configpipxtremewidget.cpp | 66 ++++++++++--------- 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index d8a495a82..c97c60899 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -151,7 +151,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) m_oplink->PairID4->setEnabled(false); m_oplink->Bind4->setEnabled(pairid4); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + qDebug() << "ConfigPipXtremeWidget: Count not read PairID field."; } UAVObjectField *pairRssiField = object->getField("PairSignalStrengths"); if (pairRssiField) { @@ -164,40 +164,44 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; + qDebug() << "ConfigPipXtremeWidget: Count not read PairID field."; } // Update the Description field // TODO use UAVObjectUtilManager::descriptionToStructure() UAVObjectField *descField = object->getField("Description"); - if (descField && descField->getValue(0) != QChar(255)) { - /* - * This looks like a binary with a description at the end: - * 4 bytes: header: "OpFw". - * 4 bytes: GIT commit tag (short version of SHA1). - * 4 bytes: Unix timestamp of compile time. - * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. - * 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded. - * 20 bytes: SHA1 sum of the firmware. - * 20 bytes: SHA1 sum of the uavo definitions. - * 20 bytes: free for now. - */ - char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; - for (unsigned int i = 0; i < 26; ++i) { - buf[i] = descField->getValue(i + 14).toChar().toLatin1(); + if (descField) { + if (descField->getValue(0) != QChar(255)) { + /* + * This looks like a binary with a description at the end: + * 4 bytes: header: "OpFw". + * 4 bytes: GIT commit tag (short version of SHA1). + * 4 bytes: Unix timestamp of compile time. + * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. + * 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded. + * 20 bytes: SHA1 sum of the firmware. + * 20 bytes: SHA1 sum of the uavo definitions. + * 20 bytes: free for now. + */ + char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; + for (unsigned int i = 0; i < 26; ++i) { + buf[i] = descField->getValue(i + 14).toChar().toLatin1(); + } + buf[26] = '\0'; + QString descstr(buf); + quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF; + for (int i = 1; i < 4; i++) { + gitDate = gitDate << 8; + gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF; + } + QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); + m_oplink->FirmwareVersion->setText(descstr + " " + date); + } else { + m_oplink->FirmwareVersion->setText(tr("Unknown")); } - buf[26] = '\0'; - QString descstr(buf); - quint32 gitDate = descField->getValue(11).toChar().toLatin1() & 0xFF; - for (int i = 1; i < 4; i++) { - gitDate = gitDate << 8; - gitDate += descField->getValue(11 - i).toChar().toLatin1() & 0xFF; - } - QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); - m_oplink->FirmwareVersion->setText(descstr + " " + date); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; - m_oplink->FirmwareVersion->setText(tr("Unknown")); + } + else { + qDebug() << "ConfigPipXtremeWidget: Failed to read Description field."; } // Update the serial number field @@ -213,7 +217,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; m_oplink->SerialNumber->setText(buf); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; + qDebug() << "ConfigPipXtremeWidget: Failed to read CPUSerial field."; } // Update the link state @@ -221,7 +225,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) if (linkField) { m_oplink->LinkState->setText(linkField->getValue().toString()); } else { - qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; + qDebug() << "ConfigPipXtremeWidget: Failed to read LinkState field."; } } From 9c7fcbe47c16e24bc211d1911e0cbbbdc2c04787 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 21:17:44 +0100 Subject: [PATCH 11/11] [OP-1105] uncrustified --- .../openpilotgcs/src/plugins/config/configpipxtremewidget.cpp | 3 +-- .../src/plugins/uavobjectutil/uavobjectutilmanager.cpp | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index c97c60899..d59fa84d0 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -199,8 +199,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) } else { m_oplink->FirmwareVersion->setText(tr("Unknown")); } - } - else { + } else { qDebug() << "ConfigPipXtremeWidget: Failed to read Description field."; } diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index a9b5afaa7..7d0edccce 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -306,6 +306,7 @@ QString UAVObjectUtilManager::getBoardDescriptionString() QByteArray arr = getBoardDescription(); int index = arr.indexOf(255); + return QString((index == -1) ? arr : arr.left(index)); }