void thread_mutex_lock_c(mutex_t *mutex, int line, char *file) { #ifdef DEBUG_MUTEXES thread_type *th = thread_self(); if (!th) LOG_WARN("No mt record for %u in lock [%s:%d]", thread_self(), file, line); LOG_DEBUG5("Locking %p (%s) on line %d in file %s by thread %d", mutex, mutex->name, line, file, th ? th->thread_id : -1); # ifdef CHECK_MUTEXES /* Just a little sanity checking to make sure that we're locking ** mutexes correctly */ if (th) { int locks = 0; avl_node *node; mutex_t *tmutex; _mutex_lock(&_mutextree_mutex); node = avl_get_first (_mutextree); while (node) { tmutex = (mutex_t *)node->key; if (tmutex->mutex_id == mutex->mutex_id) { if (tmutex->thread_id == th->thread_id) { /* Deadlock, same thread can't lock the same mutex twice */ LOG_ERROR7("DEADLOCK AVOIDED (%d == %d) on mutex [%s] in file %s line %d by thread %d [%s]", tmutex->thread_id, th->thread_id, mutex->name ? mutex->name : "undefined", file, line, th->thread_id, th->name); _mutex_unlock(&_mutextree_mutex); return; } } else if (tmutex->thread_id == th->thread_id) { /* Mutex locked by this thread (not this mutex) */ locks++; } node = avl_get_next(node); } if (locks > 0) { /* Has already got a mutex locked */ if (_multi_mutex.thread_id != th->thread_id) { /* Tries to lock two mutexes, but has not got the double mutex, norty boy! */ LOG_WARN("(%d != %d) Thread %d [%s] tries to lock a second mutex [%s] in file %s line %d, without locking double mutex!", _multi_mutex.thread_id, th->thread_id, th->thread_id, th->name, mutex->name ? mutex->name : "undefined", file, line); } } _mutex_unlock(&_mutextree_mutex); } # endif /* CHECK_MUTEXES */ _mutex_lock(mutex); _mutex_lock(&_mutextree_mutex); LOG_DEBUG2("Locked %p by thread %d", mutex, th ? th->thread_id : -1); mutex->line = line; if (th) { mutex->thread_id = th->thread_id; } _mutex_unlock(&_mutextree_mutex); #else _mutex_lock(mutex); #endif /* DEBUG_MUTEXES */ }
void thread_mutex_unlock_c(mutex_t *mutex, int line, char *file) { #ifdef DEBUG_MUTEXES thread_type *th = thread_self(); if (!th) { LOG_ERROR3("No record for %u in unlock [%s:%d]", thread_self(), file, line); } LOG_DEBUG5("Unlocking %p (%s) on line %d in file %s by thread %d", mutex, mutex->name, line, file, th ? th->thread_id : -1); mutex->line = line; # ifdef CHECK_MUTEXES if (th) { int locks = 0; avl_node *node; mutex_t *tmutex; _mutex_lock(&_mutextree_mutex); while (node) { tmutex = (mutex_t *)node->key; if (tmutex->mutex_id == mutex->mutex_id) { if (tmutex->thread_id != th->thread_id) { LOG_ERROR7("ILLEGAL UNLOCK (%d != %d) on mutex [%s] in file %s line %d by thread %d [%s]", tmutex->thread_id, th->thread_id, mutex->name ? mutex->name : "undefined", file, line, th->thread_id, th->name); _mutex_unlock(&_mutextree_mutex); return; } } else if (tmutex->thread_id == th->thread_id) { locks++; } node = avl_get_next (node); } if ((locks > 0) && (_multi_mutex.thread_id != th->thread_id)) { /* Don't have double mutex, has more than this mutex left */ LOG_WARN("(%d != %d) Thread %d [%s] tries to unlock a mutex [%s] in file %s line %d, without owning double mutex!", _multi_mutex.thread_id, th->thread_id, th->thread_id, th->name, mutex->name ? mutex->name : "undefined", file, line); } _mutex_unlock(&_mutextree_mutex); } # endif /* CHECK_MUTEXES */ _mutex_unlock(mutex); _mutex_lock(&_mutextree_mutex); LOG_DEBUG2("Unlocked %p by thread %d", mutex, th ? th->thread_id : -1); mutex->line = -1; if (mutex->thread_id == th->thread_id) { mutex->thread_id = MUTEX_STATE_NOTLOCKED; } _mutex_unlock(&_mutextree_mutex); #else _mutex_unlock(mutex); #endif /* DEBUG_MUTEXES */ }
//calibration function for mouse sensor void mouse_Calibration(int lineNb, float lineDist, int turnNb) { int i, k; RobotCommand cmd; int32 DeltaUPlus[MOUSE_NUMBER]; int32 DeltaUMinus[MOUSE_NUMBER]; int32 DeltaVPlus[MOUSE_NUMBER]; int32 DeltaVMinus[MOUSE_NUMBER]; int step = 0; mouseCalibInitStep(); //reset values of calibration for(i=0; i<MOUSE_NUMBER; i++) { uint8 code; DeltaUPlus[i] = 0; DeltaUMinus[i] = 0; DeltaVPlus[i] = 0; DeltaVMinus[i] = 0; reset(i); //set mouse in calibration mode code = MOUSE_CALIBRATION_MODE; uart_SendAll(mice[i].uart, &code, sizeof(code)); } odometrySensorUsed = MOUSE_CALIBRATION; LOG_DEBUG("Mouse Calibration"); //make the robot run N times x meters forward and backward for(k=0; k<lineNb; k++) { LOG_DEBUG1("Calibration line %d", k); //run forward motion_LineSpeedAcc(&cmd, lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION); path_LaunchTrajectory(&cmd); /*if(path_WaitEndOfTrajectory() != TRAJ_OK) { LOG_ERROR("Unable to complete calibration"); return; }*/ //wait user input LOG_DEBUG1("Calib : %4.2f m forward, click next when done", lineDist); mouseCalibWaitNextStep(step++); //accumulate values of displacement for(i=0; i<MOUSE_NUMBER; i++) { DeltaUPlus[i] += getU(i); DeltaVMinus[i] += getV(i); reset(i); } //run backward motion_LineSpeedAcc(&cmd, -lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION); path_LaunchTrajectory(&cmd); /*if(path_WaitEndOfTrajectory() != TRAJ_OK) { LOG_ERROR("Unable to complete calibration"); return; }*/ //wait user input LOG_DEBUG1("Calib : %4.2f m backward, click next when done", lineDist); mouseCalibWaitNextStep(step++); //accumulate values of displacement for(i=0; i<MOUSE_NUMBER; i++) { DeltaUMinus[i] += getU(i); DeltaVPlus[i] += getV(i); reset(i); } } //compute ratio for each mouse for(i=0; i<MOUSE_NUMBER; i++) { float theta; LOG_DEBUG5("Mouse %d => Du+ = %ld | Du- = %ld | Dv+ = %ld | Dv- = %ld", i, DeltaUPlus[i], DeltaUMinus[i], DeltaVPlus[i], DeltaVMinus[i]); mice[i].RatioKU = -DeltaUMinus[i]/(float)DeltaUPlus[i]; mice[i].RatioKV = -DeltaVPlus[i]/(float)DeltaVMinus[i]; theta = atan2f( DeltaVPlus[i]-DeltaVMinus[i], DeltaUPlus[i]-DeltaUMinus[i] ); mice[i].theta = theta; mice[i].cosTheta = cosf(mice[i].theta); mice[i].sinTheta = sinf(mice[i].theta); mice[i].Delta0 = (lineNb*lineDist) / (valueVTops * ((float)DeltaUPlus[i]*cosf(theta) - (float)DeltaVMinus[i]*sinf(theta))); LOG_DEBUG4("Ratio : coef = %f | U = %f | V = %f | Theta = %f", mice[i].Delta0, mice[i].RatioKU, mice[i].RatioKV, theta); } //make N * 2*Pi rotation LOG_DEBUG("Calibration rotation"); motion_RotateSpeedAcc(&cmd, 2*M_PI*turnNb, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION); path_LaunchTrajectory(&cmd); /*if(path_WaitEndOfTrajectory() != TRAJ_OK) { LOG_ERROR("Unable to complete calibration"); return; }*/ LOG_DEBUG1("Calib : %d turn anticlockwise, click next when done", turnNb); mouseCalibWaitNextStep(step++); //compute mouse positions for(i=0; i<MOUSE_NUMBER; i++) { int32 deltaU, deltaV; int32 deltaX, deltaY; deltaU = getU(i); deltaV = getV(i); reset(i); if(deltaU < 0) { deltaU *= mice[i].RatioKU; } if(deltaV < 0) { deltaV *= mice[i].RatioKV; } deltaX = deltaU*cosf(mice[i].theta) - deltaV*sinf(mice[i].theta); deltaY = deltaU*sinf(mice[i].theta) + deltaV*cosf(mice[i].theta); mice[i].RX0 = deltaY / (2*M_PI*turnNb); mice[i].RY0 = -deltaX / (2*M_PI*turnNb); LOG_DEBUG5("Mouse %d | dU = %ld | dV = %ld | dX = %ld | dY = %ld", i, deltaU, deltaV, deltaX, deltaY); LOG_DEBUG2("RX = %f | RY = %f", mice[i].RX0, mice[i].RY0); sendCalibParameters(i); } odometrySensorUsed = MOUSE_SENSOR; }