Working Re-Neg

source/Core/Drivers/USBPD.cpp

@@ -52,6 +52,7 @@ void    USBPowerDelivery::step() {
 }
 
 void USBPowerDelivery::PPSTimerCallback() { pe.TimersCallback(); }
+void USBPowerDelivery::reNegotiate() { pe.renegotiate(); }
 bool USBPowerDelivery::negotiationInProgress() {
   if (USBPowerDelivery::negotiationComplete()) {
     return false;
@@ -134,6 +135,10 @@ bool parseCapabilitiesArray(const uint8_t numCaps, uint8_t *bestIndex, uint16_t
   *bestIndex   = 0xFF; // Mark unselected
   *bestVoltage = 5000; // Default 5V
 
+  if (tipMeasurementDone() != 1) {
+    // Tip measurement not done, so we cannot select anything other than 5V
+    return false;
+  }
   // Fudge of 0.5 ohms to round up a little to account for us always having off periods in PWM
   uint8_t tipResistance = getTipResistanceX10();
   if (getSettingValue(SettingsOptions::USBPDMode) == usbpdMode_t::DEFAULT) {

source/Core/Drivers/USBPD.h

@@ -20,6 +20,8 @@ public:
   static uint8_t   getStateNumber();          // Debugging - Get the internal state number
   static bool      isVBUSConnected();         // Is the VBus pin connected on the FUSB302
   static uint32_t *getLastSeenCapabilities(); // returns pointer to the last seen capabilities from the powersource
+  static void      reNegotiate();
+
 private:
   //
   static int detectionState;
@@ -27,4 +29,4 @@ private:
 #endif
 
 #endif
-#endif
+#endif

source/Core/Threads/POWThread.cpp

@@ -22,12 +22,9 @@
 // Small worker thread to handle power (PD + QC) related steps
 
 void startPOWTask(void const *argument __unused) {
-
+  bool haveDoneSecondNegotiation = false;
   // Init any other misc sensors
   postRToSInit();
-  while (preStartChecksDone() == 0) {
-    osDelay(3);
-  }
   // You have to run this once we are willing to answer PD messages
   // Setting up too early can mean that we miss the ~20ms window to respond on some chargers
 #ifdef POW_PD
@@ -37,9 +34,7 @@ void startPOWTask(void const *argument __unused) {
 #endif
 #if POW_PD_EXT == 2
   FS2711::start();
-  FS2711::negotiate();
 #endif
-
   BaseType_t res;
   for (;;) {
     res = pdFALSE;
@@ -60,7 +55,13 @@ void startPOWTask(void const *argument __unused) {
     }
     USBPowerDelivery::PPSTimerCallback();
     USBPowerDelivery::step();
-
+    // Once tip measurement is done, and we have done the basic negotiation, we want to do the higher voltage negotiation steps
+    if (tipMeasurementDone() == 1 && !haveDoneSecondNegotiation) {
+      if (USBPowerDelivery::negotiationComplete()) {
+        USBPowerDelivery::reNegotiate();
+        haveDoneSecondNegotiation = true;
+      }
+    }
 #else
     (void)res;
 #endif
@@ -68,7 +69,10 @@ void startPOWTask(void const *argument __unused) {
     hub238_check_negotiation();
 #endif
 #if POW_PD_EXT == 2
-    FS2711::negotiate();
+    if (tipMeasurementDone() == 1 && !haveDoneSecondNegotiation) {
+      FS2711::negotiate();
+      haveDoneSecondNegotiation = true;
+    }
 #endif
     power_check();
   }