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;
}
