2021.02.04 / Jerry Wang

Actions:
1. Fixed DoComm send packet connector ID Field from power cabinet.

Files:
1. As follow commit history.

Image version: V1.05.XX.XXXX.XX
Image checksum: XXXXXXXX

EVSE/Projects/DD360Audi/Apps/ReDoComm.c

@@ -540,7 +540,7 @@ static int updateFirmwareHandle(uint8_t *imgName)
     return ret;
 }
 
-static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
+static int miscCommandHandle(uint8_t dataLen, uint8_t plugNum, uint8_t *data)
 {
     int ret = PASS;
     uint8_t i = 0, j = 0;
@@ -572,21 +572,21 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
 
         case MISC_CMD_OPERATIVE_STATE:
             if ((value > YES) || (value < NO)) {
-                if (id == 2) {
+                if (plugNum == 1) {
                     clearMiscCommand();
                 }
                 break;
             }
 
-            DEBUG_INFO("change availability id = %d, value = %d\r\n", id - 1, value);
-            ShmOCPP16Data->CsMsg.bits[id - 1].ChangeAvailabilityReq = YES;
+            DEBUG_INFO("change availability plugNum = %d, value = %d\r\n", plugNum, value);
+            ShmOCPP16Data->CsMsg.bits[plugNum].ChangeAvailabilityReq = YES;
             if (value == YES) {
-                strcpy((char *)ShmOCPP16Data->ChangeAvailability[id - 1].Type, "Operative");
+                strcpy((char *)ShmOCPP16Data->ChangeAvailability[plugNum].Type, "Operative");
             } else if (value == NO) {
-                strcpy((char *)ShmOCPP16Data->ChangeAvailability[id - 1].Type, "Inoperative");
+                strcpy((char *)ShmOCPP16Data->ChangeAvailability[plugNum].Type, "Inoperative");
             }
 
-            if (id == 2) {
+            if (plugNum == 1) {
                 clearMiscCommand();
             }
             break;
@@ -615,18 +615,18 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
             break;
 
         case MISC_CMD_ACCOUNT_BALANCE:
-            if (ShmSysConfigAndInfo->SysInfo.CurGunSelected != (id - 1)) {
+            if (ShmSysConfigAndInfo->SysInfo.CurGunSelected != (plugNum)) {
                 clearMiscCommand();
                 break;
             }
 
-            if (getConfirmSelectedGun(id - 1) == FAIL) {
+            if (getConfirmSelectedGun(plugNum) == FAIL) {
                 DEBUG_INFO("Remote start not select gun\r\n");
                 break;
             }
 
-            gAudiCustInfo->PricesInfo[id - 1].Balance = transPricesUnit((int)value);
-            DEBUG_INFO("%d misc balance = %.2f\r\n", id, gAudiCustInfo->PricesInfo[id - 1].Balance);
+            gAudiCustInfo->PricesInfo[plugNum].Balance = transPricesUnit((int)value);
+            DEBUG_INFO("%d misc balance = %.2f\r\n", plugNum, gAudiCustInfo->PricesInfo[plugNum].Balance);
             clearMiscCommand();
             break;
 
@@ -657,24 +657,24 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
 
         case MISC_CMD_REMOTE_START_CHARGING:
             if (value != YES) {
-                if (id == 2) {
+                if (plugNum == 1) {
                     clearMiscCommand();
                 }
                 break;
             }
-            DEBUG_INFO("Remote start charging id = %d\r\n", id);
+            DEBUG_INFO("Remote start charging plugNum = %d\r\n", plugNum);
 
-            //if (getConfirmSelectedGun(id - 1) == FAIL) {
+            //if (getConfirmSelectedGun(plugNum - 1) == FAIL) {
             //    DEBUG_INFO("Remote start not select gun\r\n");
             //    break;
             //}
 
-            if (getSelGunWaitToAuthor(id - 1) == FAIL) {
+            if (getSelGunWaitToAuthor(plugNum) == FAIL) {
                 DEBUG_ERROR("Remote start gun already charging\r\n");
                 break;
             }
 
-            if (ShmSysConfigAndInfo->SysInfo.CurGunSelected != (id - 1)) {
+            if (ShmSysConfigAndInfo->SysInfo.CurGunSelected != (plugNum)) {
                 if (ShmSysConfigAndInfo->SysInfo.CurGunSelected == LEFT_GUN_NUM &&
                         (gAudiCustInfo->SelGunInfo.LeftGun == SEL_GUN_CONFIRM ||
                          gAudiCustInfo->SelGunInfo.LeftGun == SEL_GUN_ATHOR)) {
@@ -699,20 +699,20 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
                     ShmSysConfigAndInfo->SysInfo.SystemTimeoutFlag = Timeout_None;
                 }
 
-                ShmSysConfigAndInfo->SysInfo.CurGunSelected = (id - 1);
+                ShmSysConfigAndInfo->SysInfo.CurGunSelected = (plugNum);
                 ShmSysConfigAndInfo->SysInfo.CurGunSelectedByAc = NO_DEFINE;
             }
 
             setConfirmSelGun(ShmSysConfigAndInfo->SysInfo.CurGunSelected);
 
-            ShmOCPP16Data->CsMsg.bits[id - 1].RemoteStartTransactionReq = YES;
-            gAudiCustInfo->PricesInfo[id - 1].Balance = 0.00;
+            ShmOCPP16Data->CsMsg.bits[plugNum].RemoteStartTransactionReq = YES;
+            gAudiCustInfo->PricesInfo[plugNum].Balance = 0.00;
             clearMiscCommand();
             break;
 
         case MISC_CMD_REMOTE_STOP_CHARGING:
             if (value != YES) {
-                if (id == 2) {
+                if (plugNum == 1) {
                     clearMiscCommand();
                 }
                 break;
@@ -723,14 +723,14 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
             ShmSysConfigAndInfo->SysInfo.SystemPage = _LCM_SELECT_GUN;
             gettimeofday(&ShmSysConfigAndInfo->SysInfo.SystemTimeoutTimer, NULL);
             ShmSysConfigAndInfo->SysInfo.SystemTimeoutFlag = Timeout_None;
-            destroySelectGun(id - 1);
+            destroySelectGun(plugNum);
 
             clearMiscCommand();
             break;
 
         case MISC_CMD_REMOTE_UNLOCK:
             if (value != YES) {
-                if (id == 2) {
+                if (plugNum == 1) {
                     clearMiscCommand();
                 }
                 break;
@@ -743,9 +743,9 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
                 ShmSysConfigAndInfo->SysInfo.SystemPage = _LCM_SELECT_GUN;
                 gettimeofday(&ShmSysConfigAndInfo->SysInfo.SystemTimeoutTimer, NULL);
                 ShmSysConfigAndInfo->SysInfo.SystemTimeoutFlag = Timeout_None;
-                destroySelectGun(id - 1);
+                destroySelectGun(plugNum);
             } else {
-                ChargingData[id - 1]->StopChargeFlag = YES;
+                ChargingData[plugNum]->StopChargeFlag = YES;
             }
             break;
 
@@ -759,7 +759,7 @@ static int miscCommandHandle(uint8_t dataLen, uint8_t id, uint8_t *data)
     return ret;
 }
 
-static int chargingcapabilityHandle(uint8_t *data, uint8_t gunID)
+static int chargingcapabilityHandle(uint8_t *data, uint8_t plugNum)
 {
     uint8_t addr = 0;
     float MaxVolt, MaxCurrent, MaxPower;
@@ -775,7 +775,7 @@ static int chargingcapabilityHandle(uint8_t *data, uint8_t gunID)
         if (MaxVolt > MAX_VOLTAGE) {
             MaxVolt = MAX_VOLTAGE;
         }
-        ChargingData[gunID]->MaximumChargingVoltage = MaxVolt;
+        ChargingData[plugNum]->MaximumChargingVoltage = MaxVolt;
     }
 
     MaxCurrent = (float)(ntohs(pCapabilityInfo->OutputCurrent));
@@ -783,7 +783,7 @@ static int chargingcapabilityHandle(uint8_t *data, uint8_t gunID)
         if (MaxCurrent > MAX_CURRENCY) {
             MaxCurrent = MAX_CURRENCY;
         }
-        ChargingData[gunID]->AvailableChargingCurrent = MaxCurrent;
+        ChargingData[plugNum]->AvailableChargingCurrent = MaxCurrent;
     }
 
     MaxPower = (float)(ntohs(pCapabilityInfo->OutputPower));
@@ -791,7 +791,7 @@ static int chargingcapabilityHandle(uint8_t *data, uint8_t gunID)
         if (MaxPower > MAX_POWER) {
             MaxPower = MAX_POWER;
         }
-        ChargingData[gunID]->AvailableChargingPower = MaxPower;
+        ChargingData[plugNum]->AvailableChargingPower = MaxPower;
     }
     //MaxVolt = (float)(data[0] << 8 |data[1]);
     //MaxCurrent = (float)(data[2] << 8 | data[3]);
@@ -803,20 +803,20 @@ static int chargingcapabilityHandle(uint8_t *data, uint8_t gunID)
     pAccountInfo = (AccountInfo *)&data[addr];
 
     ShmSysConfigAndInfo->SysConfig.BillingData.Currency = pAccountInfo->Currency;
-    gAudiCustInfo->PricesInfo[gunID].UserPrices         = transPricesUnit(ntohl(pAccountInfo->UserPrices));
-    ChargingData[gunID]->ChargingFee                    = transPricesUnit(ntohl(pAccountInfo->TotalCost));
-    gAudiCustInfo->PricesInfo[gunID].Balance            = transPricesUnit(ntohl(pAccountInfo->Balance));
+    gAudiCustInfo->PricesInfo[plugNum].UserPrices         = transPricesUnit(ntohl(pAccountInfo->UserPrices));
+    ChargingData[plugNum]->ChargingFee                    = transPricesUnit(ntohl(pAccountInfo->TotalCost));
+    gAudiCustInfo->PricesInfo[plugNum].Balance            = transPricesUnit(ntohl(pAccountInfo->Balance));
 
-    if ((pricesInfo[gunID].UserPrices != gAudiCustInfo->PricesInfo[gunID].UserPrices) ||
-            (pricesInfo[gunID].Balance != gAudiCustInfo->PricesInfo[gunID].Balance)) {
-        pricesInfo[gunID].UserPrices = gAudiCustInfo->PricesInfo[gunID].UserPrices;
-        pricesInfo[gunID].Balance = gAudiCustInfo->PricesInfo[gunID].Balance;
+    if ((pricesInfo[plugNum].UserPrices != gAudiCustInfo->PricesInfo[plugNum].UserPrices) ||
+            (pricesInfo[plugNum].Balance != gAudiCustInfo->PricesInfo[plugNum].Balance)) {
+        pricesInfo[plugNum].UserPrices = gAudiCustInfo->PricesInfo[plugNum].UserPrices;
+        pricesInfo[plugNum].Balance = gAudiCustInfo->PricesInfo[plugNum].Balance;
 
         DEBUG_INFO("id = %d, user prices = %.2f, Total cost = %.2f, Account balances = %.2f, currency = %s\r\n",
-                   gunID,
-                   gAudiCustInfo->PricesInfo[gunID].UserPrices,
-                   ChargingData[gunID]->ChargingFee,
-                   gAudiCustInfo->PricesInfo[gunID].Balance,
+                   plugNum,
+                   gAudiCustInfo->PricesInfo[plugNum].UserPrices,
+                   ChargingData[plugNum]->ChargingFee,
+                   gAudiCustInfo->PricesInfo[plugNum].Balance,
                    (uint8_t *)Currency[ShmSysConfigAndInfo->SysConfig.BillingData.Currency]);
     }
 
@@ -995,7 +995,7 @@ static int powerCabinetStatusProcess(uint8_t dataLen, uint8_t *data)
     return PASS;
 }
 
-static int responsePackeHandle(int fd, uint8_t *pResult, uint8_t id, uint8_t reg)
+static int responsePackeHandle(int fd, uint8_t *pResult, uint8_t plugNum, uint8_t reg)
 {
     int ret = PASS;
     uint8_t rawDataLen = 0;
@@ -1037,7 +1037,7 @@ static int responsePackeHandle(int fd, uint8_t *pResult, uint8_t id, uint8_t reg
         break;
 
     case REG_CHARGING_CAP:
-        chargingcapabilityHandle(pCsuResult->Data.Data, id - 1);
+        chargingcapabilityHandle(pCsuResult->Data.Data, plugNum);
         break;
 
     case REG_CHARGING_TARGET:
@@ -1086,7 +1086,7 @@ static int responsePackeHandle(int fd, uint8_t *pResult, uint8_t id, uint8_t reg
         break;
 
     case REG_MISC_CONTROL:
-        miscCommandHandle(rawDataLen, pCsuResult->Head.ID, pCsuResult->Data.Data);
+        miscCommandHandle(rawDataLen, plugNum, pCsuResult->Data.Data);
         break;
 
     case REG_REPORT_CSU_VERSION:
@@ -1120,6 +1120,8 @@ static int composeSocketData(int fd,
     int ret = PASS;
     int size = 0;
     int sendPktLen = 0;
+    uint8_t i = 0;
+    uint8_t plugNum = 0;
     CsuCmdPkt csuCmdPkt = {0};
     CsuResultPkt csuResult = {0};
 
@@ -1158,7 +1160,14 @@ static int composeSocketData(int fd,
         return FAIL;
     }
 
-    ret = responsePackeHandle(fd, (uint8_t *)&csuResult, id, csuCmdPkt.Data.Register);
+    for (i = 0; i < sizeof(gDoCommGblData.ConnectorID); i++) {
+        if (id == gDoCommGblData.ConnectorID[i]) {
+            plugNum = i;
+            break;
+        }
+    }
+
+    ret = responsePackeHandle(fd, (uint8_t *)&csuResult, plugNum, csuCmdPkt.Data.Register);
 
     return ret;
 }
@@ -1358,13 +1367,13 @@ static int writePlugInStatus(int fd, uint8_t plugNum, uint8_t id)
     return ret;
 }
 
-static int readSoftwareUpdate(int fd)
+static int readSoftwareUpdate(int fd, uint8_t gunID)
 {
     int ret = PASS;
     uint8_t dataBuf[2] = {ShmSysConfigAndInfo->SysInfo.FirmwareUpdate, 0};
 
     ret = composeSocketData(fd,
-                            0x01,
+                            gunID,
                             OP_READ_DATA,
                             REG_SOFTWARE_UPDATE,
                             1,
@@ -1413,7 +1422,7 @@ static int readChargingCapability(int fd, uint8_t id)
     return ret;
 }
 
-static int writeDispenserStatus(int fd)
+static int writeDispenserStatus(int fd, uint8_t gunID)
 {
     uint8_t warningCount = 0;
     uint8_t count = 0;
@@ -1441,7 +1450,7 @@ static int writeDispenserStatus(int fd)
     }
 
     ret = composeSocketData(fd,
-                            ID_OF_DISPENSER,
+                            gunID,
                             OP_WRITE_DATA,
                             REG_DISPENSER_STATUS,
                             strlen((char *)CurWarnCodeTmp),
@@ -1450,12 +1459,12 @@ static int writeDispenserStatus(int fd)
     return ret;
 }
 
-static int readPowerCabinetStatus(int fd)
+static int readPowerCabinetStatus(int fd, uint8_t gunID)
 {
     int ret = PASS;
 
     ret = composeSocketData(fd,
-                            ID_OF_DISPENSER,
+                            gunID,
                             OP_READ_DATA,
                             REG_POWER_CABINET_STATUS,
                             0,
@@ -1614,7 +1623,7 @@ static int InitShareMemory(void)
     return result;
 }
 
-static void updateFirmwareProcess(int fd, uint8_t totalConnCount)
+static void updateFirmwareProcess(int fd, uint8_t gunID, uint8_t totalConnCount)
 {
     bool canUpdateFirmware = true;
     uint8_t plugNum = 0;
@@ -1634,7 +1643,7 @@ static void updateFirmwareProcess(int fd, uint8_t totalConnCount)
     if (canUpdateFirmware) {
         ftime(&updateTime);
         if (DiffTimeb(gRegTimeUp[0][REG_SOFTWARE_UPDATE], updateTime) > LOOP_RETRY_TIME * 5) {
-            readSoftwareUpdate(fd);
+            readSoftwareUpdate(fd, gunID);
             ftime(&gRegTimeUp[0][REG_SOFTWARE_UPDATE]);
         }
 
@@ -1642,7 +1651,7 @@ static void updateFirmwareProcess(int fd, uint8_t totalConnCount)
             while (ackCount != 0) {
                 ftime(&updateTime);
                 if (DiffTimeb(gRegTimeUp[0][REG_SOFTWARE_UPDATE], updateTime) > LOOP_RETRY_TIME / 2) {
-                    readSoftwareUpdate(fd);
+                    readSoftwareUpdate(fd, gunID);
                     ftime(&gRegTimeUp[0][REG_SOFTWARE_UPDATE]);
                     if (ShmSysConfigAndInfo->SysInfo.FirmwareUpdate == NO) {
                         ackCount--;
@@ -1657,14 +1666,17 @@ static void updateFirmwareProcess(int fd, uint8_t totalConnCount)
 static void checkAuthorProcess(int fd, uint8_t plugNum)
 {
     int ret = 0;
+    uint8_t gunID = 0;
     struct timeb AuthNowTime;
 
+    gunID = gDoCommGblData.ConnectorID[ShmSysConfigAndInfo->SysInfo.CurGunSelected];
+
     if ((ShmOCPP16Data->SpMsg.bits.AuthorizeReq == YES) || (ShmSysConfigAndInfo->SysInfo.AuthorizeFlag == YES)) {
         ftime(&AuthNowTime);
 
         if (DiffTimeb(gRegTimeUp[plugNum][REG_USER_ID], AuthNowTime) > LOOP_RETRY_TIME) {
             ret = writeUserID(fd,
-                              ShmSysConfigAndInfo->SysInfo.CurGunSelected + 1,
+                              gunID,
                               ShmSysConfigAndInfo->SysConfig.UserId);
             if (ret >= 0) {
                 memset(ShmOCPP16Data->Authorize.ResponseIdTagInfo.Status,
@@ -1707,6 +1719,8 @@ static int messageTrigger(int fd, uint8_t plugNum, uint8_t gunID, uint8_t curReg
             if (WriteModelName(fd) == PASS) {
                 gDoCommGblData.DisConnCount = 0;
                 curReg = REG_CONNECTOR_ID;
+            } else {
+                sleep(1);
             }
             break;
 
@@ -1714,12 +1728,14 @@ static int messageTrigger(int fd, uint8_t plugNum, uint8_t gunID, uint8_t curReg
             if (readConnectorID(fd) == PASS) {
                 gDoCommGblData.DisConnCount = 0;
                 curReg = REG_REPORT_CSU_VERSION;
+            } else {
+                sleep(1);
             }
             break;
 
         case REG_POWER_CABINET_STATUS:
             if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME) {
-                readPowerCabinetStatus(fd);
+                readPowerCabinetStatus(fd, gunID);
                 ftime(&gRegTimeUp[plugNum][curReg]);
             }
             curReg = REG_DISPENSER_STATUS;
@@ -1727,7 +1743,7 @@ static int messageTrigger(int fd, uint8_t plugNum, uint8_t gunID, uint8_t curReg
 
         case REG_DISPENSER_STATUS:
             if (DiffTimeb(gRegTimeUp[plugNum][curReg], NowTime) > LOOP_RETRY_TIME) {
-                writeDispenserStatus(fd);
+                writeDispenserStatus(fd, gunID);
                 ftime(&gRegTimeUp[plugNum][curReg]);
             }
             curReg = REG_PLUG_IN_STATE;
@@ -1858,8 +1874,9 @@ static int messageTrigger(int fd, uint8_t plugNum, uint8_t gunID, uint8_t curReg
 //    return NO;
 //}
 
-static void systemStatusProcess(int fd, uint8_t plugNum, uint8_t gunID)
+static void systemStatusProcess(int fd, uint8_t totalGun, uint8_t plugNum, uint8_t gunID)
 {
+    uint8_t i = 0;
     struct timeb AuthNowTime = {0};
 
     switch (ChargingData[plugNum]->SystemStatus) {
@@ -1896,8 +1913,9 @@ static void systemStatusProcess(int fd, uint8_t plugNum, uint8_t gunID)
         ftime(&AuthNowTime);
         if (DiffTimeb(gRegTimeUp[plugNum][REG_CHARGING_PERMISSION], AuthNowTime) > LOOP_RETRY_TIME) {
             if (readChargePermission(fd, gunID) && readChargingCapability(fd, gunID)) {
-                ShmPsuData->SystemAvailablePower = (ChargingData[0]->AvailableChargingPower +
-                                                    ChargingData[1]->AvailableChargingPower);
+                for (i = 0; i < totalGun; i++) {
+                    ShmPsuData->SystemAvailablePower += ChargingData[i]->AvailableChargingPower;
+                }
                 ShmPsuData->SystemPresentPsuQuantity = (ShmPsuData->SystemAvailablePower / 30);
             }
             ftime(&gRegTimeUp[plugNum][REG_CHARGING_PERMISSION]);
@@ -2097,8 +2115,9 @@ int main(int argc, char *argv[])
             gunID = 0;
             for (plugNum = 0; plugNum < totalConnCount; plugNum++) {
                 //plugNum = setup chargingData value for bottom layer
-                gunID = (plugNum  + 1); //connector Id, 1 = left gun, 2 = right gun
-                systemStatusProcess(fd, plugNum, gunID);
+                //gunID = (plugNum  + 1); //connector Id, 1 = left gun, 2 = right gun
+                gunID = gDoCommGblData.ConnectorID[plugNum];
+                systemStatusProcess(fd, totalConnCount, plugNum, gunID);
 
                 initDone = messageTrigger(fd,
                                           plugNum,
@@ -2126,7 +2145,7 @@ int main(int argc, char *argv[])
 
         //update dispenser firmware
         if (initDone != DISPENER_INIT_FAIL) {
-            updateFirmwareProcess(fd, totalConnCount);
+            updateFirmwareProcess(fd, gDoCommGblData.ConnectorID[0], totalConnCount);
         }
 
         usleep(1000);

EVSE/Projects/DD360Audi/Apps/ReDoComm.h

@@ -114,12 +114,18 @@
 #pragma pack(push)
 #pragma pack(1)
 
+typedef struct StConnectorIDTable {
+    uint8_t DcConnID1;
+    uint8_t DcConnID2;
+    uint8_t AcConnID;
+} ConnectorIDTable;
+
 typedef struct StDoCommGlobalVar {
     uint8_t SeqNum;
     uint8_t DisConnCount;
-    uint8_t ConnectorID[2]; //keep from power cabinet
+    uint8_t ConnectorID[3]; //keep from power cabinet
     uint8_t MiscCmd;
-    uint8_t Reserved[3];
+    uint8_t Reserved[2];
 } DoCommGblData;
 
 typedef struct StCommnadHead {

EVSE/Projects/DD360Audi/Apps/main.c

@@ -2653,6 +2653,7 @@ void CheckGunTypeFromHw()
         case 23:
             bd1_2_status = gpioValue;
             break;
+
         case 44:
             bd0_1_status = gpioValue;
             break;
@@ -2671,16 +2672,16 @@ void CheckGunTypeFromHw()
     for (int i = 0; i < 2; i++) {
         switch (tmp[i]) {
         case 0x01:
-            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i == 1 ? "Right" : "Left", "CHAdeMO");
+            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i + 1 == 1 ? "Right" : "Left", "CHAdeMO");
             break;
         case 0x10:
-            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i == 1 ? "Right" : "Left", "CCS");
+            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i + 1 == 1 ? "Right" : "Left", "CCS");
             break;
         case 0x11:
-            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i == 1 ? "Right" : "Left", "GBT");
+            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i + 1 == 1 ? "Right" : "Left", "GBT");
             break;
         case 0x00:
-            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i == 1 ? "Right" : "Left", "None");
+            DEBUG_INFO_MSG("BD%d(%s) = %s ", i + 1, i + 1 == 1 ? "Right" : "Left", "None");
             break;
         }
     }