diff --git a/euler.h b/euler.h index a694adb..4ac6ac5 100644 --- a/euler.h +++ b/euler.h @@ -29,6 +29,6 @@ float q_to_roll(float r, float i, float j, float k); // Get Yaw, Pitch and Roll from quaternion void q_to_ypr(float r, float i, float j, float k, - float *pRoll, float *pPitch, float *pYaw); + float *pYaw, float *pPitch, float *pRoll); #endif diff --git a/sh2.c b/sh2.c index 39d055f..cc4e424 100644 --- a/sh2.c +++ b/sh2.c @@ -887,9 +887,11 @@ static void getProdIdRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len) } // Complete this operation if there is no storage for more product ids - if ((pSh2->opData.getProdIds.pProdIds == 0) || - (pSh2->opData.getProdIds.nextEntry >= pSh2->opData.getProdIds.expectedEntries)) { - + if (pSh2->opData.getProdIds.pProdIds == 0){ + opCompleted(pSh2, SH2_OK); + return; + } + if (pSh2->opData.getProdIds.nextEntry >= pSh2->opData.getProdIds.expectedEntries) { pSh2->opData.getProdIds.pProdIds->numEntries = pSh2->opData.getProdIds.nextEntry; opCompleted(pSh2, SH2_OK); } diff --git a/shtp.c b/shtp.c index ebce552..f372a82 100644 --- a/shtp.c +++ b/shtp.c @@ -198,6 +198,17 @@ static void rxAssemble(shtp_t *pShtp, uint8_t *in, uint16_t len, uint32_t t_us) chan = in[2]; seq = in[3]; + if (chan >= SHTP_MAX_CHANS) { + // Invalid channel id. + pShtp->rxBadChan++; + + if (pShtp->eventCallback) { + pShtp->eventCallback(pShtp->eventCookie, SHTP_BAD_RX_CHAN); + } + return; + } + + if (seq != pShtp->chan[chan].nextInSeq){ if (pShtp->eventCallback) { pShtp->eventCallback(pShtp->eventCookie, @@ -212,16 +223,6 @@ static void rxAssemble(shtp_t *pShtp, uint8_t *in, uint16_t len, uint32_t t_us) } return; } - - if (chan >= SHTP_MAX_CHANS) { - // Invalid channel id. - pShtp->rxBadChan++; - - if (pShtp->eventCallback) { - pShtp->eventCallback(pShtp->eventCookie, SHTP_BAD_RX_CHAN); - } - return; - } // Discard earlier assembly in progress if the received data doesn't match it. if (pShtp->inRemaining) {