Merge remote-tracking branch 'origin/LesNewell/OP-984_PID_banks' into thread/OP-984_PID_banks_GUI

Conflicts:
	ground/openpilotgcs/src/plugins/config/stabilization.ui

flight/modules/ManualControl/manualcontrol.c

@@ -690,46 +690,53 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
         return;
     }
 
-    // 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) ?
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING)  ? cmd->Roll * stabSettings.ManualRate.Roll :
-        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) ?
+        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE)     ? cmd->Roll :
-        cmd->Roll * stabSettings.ManualRate.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
+        (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) ?
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING)  ? cmd->Pitch * stabSettings.ManualRate.Pitch :
-        cmd->Pitch * stabSettings.ManualRate.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)      ? cmd->Pitch * stabSettings.PitchMax :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK)      ? cmd->Pitch * stabSettings.ManualRate.Pitch :
-        cmd->Pitch * stabSettings.ManualRate.Pitch :
         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR)    ? cmd->Pitch :
-        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
+        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE)     ? cmd->Pitch :
-        cmd->Pitch * stabSettings.ManualRate.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
+        (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 Rattitude, so use Rate
+    // Should really do this for Attitude mode as well?
+    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) ?
+            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING)  ? cmd->Yaw * stabSettings.ManualRate.Yaw :
-        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
+            (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);

flight/modules/Stabilization/stabilization.c

@@ -75,11 +75,14 @@
 #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 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 };
 enum{RATE_P, RATE_I, RATE_D, RATE_LIMIT, RATE_OFFSET};
 enum{ATT_P, ATT_I, ATT_LIMIT, ATT_OFFSET};
 
-
 // Private variables
 static xTaskHandle taskHandle;
 static StabilizationSettingsData settings;
@@ -94,8 +97,11 @@ bool lowThrottleZeroIntegral;
 bool lowThrottleZeroAxis[MAX_AXES];
 float vbar_decay = 0.991f;
 struct pid pids[PID_MAX];
+
 int flight_mode = -1;
 
+static uint8_t rattitude_anti_windup;
+
 // Private functions
 static void stabilizationTask(void *parameters);
 static float bound(float val, float range);
@@ -104,6 +110,9 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
 static void BankUpdatedCb(UAVObjEvent *ev);
 static void SettingsBankUpdatedCb(UAVObjEvent *ev);
 
+static float stab_log2f(float x);
+static float stab_powf(float x, uint8_t p);
+
 /**
  * Module initialization
  */
@@ -197,7 +206,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 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;
@@ -292,7 +301,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;
 
@@ -317,7 +326,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 
                 // Store to rate desired variable for storing to UAVO
                 rateDesiredAxis[i] =
-                    bound(attitudeDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
+                    bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
 
                 // Compute the inner loop
                 actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@@ -342,10 +351,127 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
 
                 break;
 
+            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
+            // - 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;
+                    pids[PID_RATEA_ROLL + i].iAccumulator = 0;
+                }
+
+                // Compute what Rate mode would give for this stick angle'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(stabBank.ManualRate, stabBank.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(stabBank.RollMax, stabBank.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(stabBank.MaximumRate,
+                                                                     stabBank.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 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);
+
+                // 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
+
+                // 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 must never increase the iAcc or we risk oscillation.
+
+                // 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
+                // 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
+
+                // magic numbers
+                // these generate the inverted parabola like curves that go through [0,1] and [1,0]
+                // 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) {
+                    float factor;
+
+                    // At magnitudes close to one, the Attitude accumulators gets zeroed
+                    if (pids[PID_ROLL+i].i > 0.0f) {
+                        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 - 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 - 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;
+                    }
+                }
+
+                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);
@@ -353,6 +479,12 @@ 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
+            // Also does not take dT into account
             {
                 if (reinit) {
                     pids[PID_RATE_ROLL + i].iAccumulator = 0;
@@ -362,7 +494,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);
 
@@ -374,13 +506,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);
                 }
@@ -395,7 +527,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(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
 
                 // Run the relay controller which also estimates the oscillation parameters
@@ -421,7 +553,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;
@@ -514,6 +646,44 @@ static float bound(float val, float range)
 }
 
 
+// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
+static float stab_log2f(float x)
+{
+  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 SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
@@ -541,7 +711,7 @@ static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 //        return; //bank number is invalid. All we can do is ignore it.
     }
 
-//Need to do this to prevent an infinite loop
+	//Need to do this to prevent an infinite loop
     if(memcmp(&oldBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
     {
         StabilizationBankSet(&bank);
@@ -625,6 +795,27 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
                   bank.YawPI.Ki,
                   0,
                   bank.YawPI.ILimit);
+
+    // Set the Rattitude roll rate PID constants
+    pid_configure(&pids[PID_RATEA_ROLL],
+                  bank.RollRatePID.Kp,
+                  bank.RollRatePID.Ki,
+                  bank.RollRatePID.Kd,
+                  bank.RollRatePID.ILimit);
+
+    // Set the Rattitude pitch rate PID constants
+    pid_configure(&pids[PID_RATEA_PITCH],
+                  bank.PitchRatePID.Kp,
+                  bank.PitchRatePID.Ki,
+                  bank.PitchRatePID.Kd,
+                  bank.PitchRatePID.ILimit);
+
+    // Set the Rattitude yaw rate PID constants
+    pid_configure(&pids[PID_RATEA_YAW],
+                  bank.YawRatePID.Kp,
+                  bank.YawRatePID.Ki,
+                  bank.YawRatePID.Kd,
+                  bank.YawRatePID.ILimit);
 }
 
 
@@ -666,9 +857,12 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     // Compute time constant for vbar decay term based on a tau
     vbar_decay = expf(-fakeDt / settings.VbarTau);
 
-    flight_mode = -1; //force flight mode update
+    //force flight mode update
-}
+    flight_mode = -1;
 
+    // Rattitude flight mode anti-windup factor
+    rattitude_anti_windup = settings.RattitudeAntiWindup;
+}
 
 /**
  * @}

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,12 +164,14 @@ 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) {
+        if (descField->getValue(0) != QChar(255)) {
             /*
              * This looks like a binary with a description at the end:
              *   4 bytes: header: "OpFw".
@@ -195,7 +197,10 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
             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
@@ -211,7 +216,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
@@ -219,7 +224,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.";
     }
 }
 

ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp

@@ -301,6 +301,14 @@ 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 +482,3 @@ bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescrip
     }
     return false;
 }
-
-// ******************************

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();

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(QPixmap(":uploader/images/view-refresh.svg"));
-    myDevice->statusIcon->setPixmap(pix);
 
     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->lblAccess->setText(tr("Flash access: ") + QString(r ? "R" : "-") + QString(w ? "W" : "-"));
-    myDevice->lblMaxCode->setText(QString("Max code size: ") + QString::number(m_dfu->devices[deviceID].SizeOfCode));
+    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))));
+        myDevice->lblDescription->setText((!str.isEmpty()) ? str : tr("Unknown"));
-        QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
+        myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
-        myDevice->lblCertified->setPixmap(pix);
         myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
-        myDevice->lblBuildDate->setText("Warning: development firmware");
+        myDevice->lblBuildDate->setText(tr("Unknown"));
-        myDevice->lblGitTag->setText("");
+        myDevice->lblGitTag->setText(tr("Unknown"));
-        myDevice->lblBrdName->setText("");
+        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(QPixmap(":uploader/images/application-certificate.svg"));
-            myDevice->lblCertified->setPixmap(pix);
             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(QPixmap(":uploader/images/warning.svg"));
-            myDevice->lblCertified->setPixmap(pix);
             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(QPixmap(":uploader/images/application-certificate.svg"));
-            myDevice->lblCertifiedL->setPixmap(pix);
             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(QPixmap(":uploader/images/warning.svg"));
-            myDevice->lblCertifiedL->setPixmap(pix);
             myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build"));
         }
         myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescription.boardType << 8 | LoadedDescription.boardRevision));

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;
 }
 

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);
+            myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag);
-            QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
+            myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/application-certificate.svg"));
-            myDevice->lblCertified->setPixmap(pix);
             myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
         } else {
-            myDevice->lblFWTag->setText(QString("Firmware tag: ") + devDesc.gitTag);
+            myDevice->lblFWTag->setText(tr("Firmware tag: ") + devDesc.gitTag);
-            QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
+            myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
-            myDevice->lblCertified->setPixmap(pix);
             myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
         }
-        myDevice->lblGitCommitTag->setText("Git commit hash: " + devDesc.gitHash);
+        myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + devDesc.gitHash);
-        myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.gitDate.insert(4, "-").insert(7, "-"));
+        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))));
+        QString desc = utilMngr->getBoardDescriptionString();
-        myDevice->lblGitCommitTag->setText("Git commit tag: Unknown");
+        myDevice->lblFWTag->setText(tr("Firmware tag: ") + (!desc.isEmpty() ? desc : tr("Unknown")));
-        myDevice->lblFWDate->setText(QString("Firmware date: Unknown"));
+        myDevice->lblCertified->setPixmap(QPixmap(":uploader/images/warning.svg"));
-        QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
-        myDevice->lblCertified->setPixmap(pix);
         myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
+        myDevice->lblGitCommitTag->setText(tr("Git commit hash: ") + tr("Unknown"));
+        myDevice->lblFWDate->setText(tr("Firmware date: ") + tr("Unknown"));
     }
 }

shared/uavobjectdefinition/manualcontrolsettings.xml

@@ -22,17 +22,17 @@
 	<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
         <field name="Stabilization1Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw"
-		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"
+		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
 		defaultvalue="Attitude,Attitude,AxisLock"
 		limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
         <field name="Stabilization2Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw"
-		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"
+		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
 		defaultvalue="Attitude,Attitude,Rate"
 		limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
         <field name="Stabilization3Settings" units="" type="enum"
 		elementnames="Roll,Pitch,Yaw"
-		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"
+		options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"
 		defaultvalue="Rate,Rate,Rate"
 		limits="%NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude; %NE:RelayRate:RelayAttitude;"/>
 

shared/uavobjectdefinition/stabilizationdesired.xml

@@ -6,7 +6,7 @@
         <field name="Yaw" units="degrees" type="float" elements="1"/>
         <field name="Throttle" units="%" type="float" elements="1"/>
 	<!-- These values should match those in ManualControlCommand.Stabilization{1,2,3}Settings -->
-        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,RelayRate,RelayAttitude"/>
+        <field name="StabilizationMode" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>

shared/uavobjectdefinition/stabilizationsettings.xml

@@ -28,6 +28,8 @@
 	<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
 	<field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
 
+	<field name="RattitudeAntiWindup" units="" type="uint8" elements="1" defaultvalue="10"/>
+
 	<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
 
 	<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>