int Mutex_Lock(IN HANDLE hm, IN DWORD dwWaitTimeout) { MUTEX *pMutex = (MUTEX *)hm; struct timespec ts; long nWaitSeconds, nWaitNonaseconds; time_t tmEndTime; if (hm == NULL) { return ERR_INVALID_ARGS; } nWaitSeconds = (long)MS_TO_SEC(dwWaitTimeout); // sec nWaitNonaseconds = (long)MS_TO_NS(dwWaitTimeout%MS_PER_SEC); // nonasec clock_gettime(CLOCK_REALTIME, &ts); // get the start time. ts.tv_nsec += nWaitNonaseconds; if (ts.tv_nsec > NS_PER_SEC) { ts.tv_sec += 1; ts.tv_nsec -= NS_PER_SEC; } tmEndTime = ts.tv_sec; #ifdef _DEBUG_MUTEX TRACE("[Mutex_Lock] -- lock and wait %d ms at %d\n", (int)dwWaitTimeout, (int)tmEndTime); #endif //_DEBUG_MUTEX while (nWaitSeconds >= 0) { // get the end of the real time. tmEndTime += ((nWaitSeconds < MAX_WAIT_INTERVAL) ? nWaitSeconds : MAX_WAIT_INTERVAL); ts.tv_sec = tmEndTime; // do NOT change ts.tv_nsec. if (pthread_mutex_timedlock(&pMutex->hInternalMutex, &ts) == 0) { return ERR_MUTEX_OK; } nWaitSeconds -= MAX_WAIT_INTERVAL; if (nWaitSeconds >= 0) { RUN_THREAD_HEARTBEAT(); } } #ifdef _DEBUG_MUTEX TRACE("[Mutex_Lock] -- lock timeouted at %d\n", (int)time(NULL)); #endif //_DEBUG_MUTEX // if goes here, timeout on trying to lock. return ERR_MUTEX_TIMEOUT; }
/********************************************************************** * Function: updateHeading * @return None * @remark Determines the heading error using the tilt-compensated compass, * and adjusts the rudder accordingly. Also, a bang-bang control has been * implemented to turn the rudder to the maximum value if the boat's motors * are being driven below some percentage defined above. * @author Darrel Deo * @author David Goodman * @date 2013.03.27 **********************************************************************/ static void updateRudder() { static int16_t lastThetaError; // used for derivative term rudderDirection = RUDDER_TURN_LEFT; // Get current heading, determine theta error uint16_t currentHeading = (uint16_t)TiltCompass_getHeading(); int16_t thetaError = desiredHeading - currentHeading; // Bound theta error and determine turn direction if (thetaError > 0) { rudderDirection = (thetaError < 180)? RUDDER_TURN_RIGHT : RUDDER_TURN_LEFT; thetaError = (thetaError < 180)? thetaError : (360 - thetaError); } else { // theta error is negative rudderDirection = (thetaError > -180)? RUDDER_TURN_LEFT : RUDDER_TURN_RIGHT; thetaError = (thetaError > -180)? -thetaError : (360 + thetaError); } // Initialize or dump derivative if changed directions if (lastRudderDirection == RUDDER_TURN_NONE || rudderDirection != lastRudderDirection) lastThetaError = 0; /* Controller Terms */ // Derivative (scaled by KD_RUDDER) float thetaErrorDerivative = (float)abs(thetaError - lastThetaError)/MS_TO_SEC(TRACK_UPDATE_DELAY); // Proportional (scaled by KP_RUDDER), convert degrees to percent float uDegrees = KP_RUDDER*(float)thetaError + KD_RUDDER*thetaErrorDerivative; float uPercent = (uDegrees / RUDDER_ANGLE_MAX ) * 100; // Limit percentage from 0 to 100 uPercent = (uPercent > 100.0)? 100.0f : uPercent; uPercent = (uPercent < 0.0)? 0.0f : uPercent; char *bangbang = ""; // Bang-bang control to force rudder all the way if speed is low if (desiredSpeed < RUDDER_BANGBANG_SPEED_THRESHOLD && thetaError > RUDDER_BANGBANG_THETA_DEADBAND_THRESHOLD) { uPercent = 100.0f; bangbang = " BB"; } // Command the rudder and save setRudder(rudderDirection, (uint8_t)uPercent); lastThetaError = thetaError; lastRudderDirection = rudderDirection; char *dir; dir = (rudderDirection == RUDDER_TURN_RIGHT)? "R" : "L"; #ifdef DEBUG_VERBOSE DBPRINT("Rudder control: rDegrees=%d, yDegrees=%d, eDegrees=%d, uDegrees=%.2f, uPercent=%d[%s]\n\n", desiredHeading, currentHeading, thetaError, uDegrees, (uint8_t)uPercent, dir); #endif #ifdef USE_PUBLIC_DEBUG sprintf(debugString, "R=%d, Y=%d, e=%d, U=%.2f, Up=%d[%s]%s, S=%d\n", desiredHeading, currentHeading, thetaError, uDegrees, (uint8_t)uPercent, dir, bangbang, desiredSpeed); #endif }
/*==========================================================================* * FUNCTION : Sem_Wait * PURPOSE : * CALLS : * CALLED BY: * ARGUMENTS: IN HANDLE hSem : * IN DWORD dwWaitTimeout : * RETURN : int : * COMMENTS : *==========================================================================*/ int Sem_Wait(IN HANDLE hSem, IN DWORD dwWaitTimeout) { SEMAPHORE *pSem = (SEMAPHORE *)hSem; struct timespec ts; long nWaitSeconds, nWaitNonaseconds; time_t tmEndTime; if (hSem == NULL) { return ERR_INVALID_ARGS; } nWaitSeconds = (long)MS_TO_SEC(dwWaitTimeout); // sec nWaitNonaseconds = (long)MS_TO_NS(dwWaitTimeout%MS_PER_SEC); // nonasec clock_gettime(CLOCK_REALTIME, &ts); // get the start time. ts.tv_nsec += nWaitNonaseconds; if (ts.tv_nsec > NS_PER_SEC) { ts.tv_sec += 1; ts.tv_nsec -= NS_PER_SEC; } tmEndTime = ts.tv_sec; #ifdef _DEBUG_MUTEX TRACE("[Sem_Wait] -- %p wait %d ms at %d\n", RunThread_GetId(NULL), (int)dwWaitTimeout, (int)tmEndTime); #endif //_DEBUG_MUTEX while (nWaitSeconds >= 0) { // get the end of the real time. tmEndTime += ((nWaitSeconds < MAX_WAIT_INTERVAL) ? nWaitSeconds : MAX_WAIT_INTERVAL); ts.tv_sec = tmEndTime; // do NOT change ts.tv_nsec. if (sem_timedwait(&pSem->hInternalSem, &ts) == 0) { return ERR_MUTEX_OK;//wait ok } //continue to try to wait. nWaitSeconds -= MAX_WAIT_INTERVAL; if (nWaitSeconds >= 0) { RUN_THREAD_HEARTBEAT(); } } #ifdef _DEBUG_MUTEX TRACE("[Sem_Wait] -- wait timeouted at %d\n", (int)time(NULL)); #endif //_DEBUG_MUTEX // if goes here, timeout on trying to lock. return ERR_MUTEX_TIMEOUT; }