Working Re-Neg

source/Core/Drivers/USBPD.cpp

@@ -135,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/Threads/POWThread.cpp

@@ -22,7 +22,7 @@
 // Small worker thread to handle power (PD + QC) related steps
 
 void startPOWTask(void const *argument __unused) {
-  bool haveNegotiatedHigherPower = false;
+  bool haveDoneSecondNegotiation = false;
   // Init any other misc sensors
   postRToSInit();
   // You have to run this once we are willing to answer PD messages
@@ -56,8 +56,11 @@ 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 && !haveNegotiatedHigherPower) {
-      USBPowerDelivery::reNegotiate();
+    if (tipMeasurementDone() == 1 && !haveDoneSecondNegotiation) {
+      if (USBPowerDelivery::negotiationComplete()) {
+        USBPowerDelivery::reNegotiate();
+        haveDoneSecondNegotiation = true;
+      }
     }
 #else
     (void)res;
@@ -66,8 +69,9 @@ void startPOWTask(void const *argument __unused) {
     hub238_check_negotiation();
 #endif
 #if POW_PD_EXT == 2
-    if (tipMeasurementDone() == 1) {
+    if (tipMeasurementDone() == 1 && !haveDoneSecondNegotiation) {
       FS2711::negotiate();
+      haveDoneSecondNegotiation = true;
     }
 #endif
     power_check();