void pidController()
{
while( true )
{
int flTargetPower = controlRequestedPitch + controlRequestedRoll;
int frTargetPower = controlRequestedPitch - controlRequestedRoll;
int rlTargetPower = -controlRequestedPitch + controlRequestedRoll;
int rrTargetPower = -controlRequestedPitch - controlRequestedRoll;
m.lock();
if( flTargetPower < 0 )
flMotorPower = 0;
else
flMotorPower = flTargetPower;
if( frTargetPower < 0 )
frMotorPower = 0;
else
frMotorPower = frTargetPower;
if( rlTargetPower < 0 )
rlMotorPower = 0;
else
rlMotorPower = rlTargetPower;
if( rrTargetPower < 0 )
rrMotorPower = 0;
else
rrMotorPower = rrTargetPower;
m.unlock();
usleep( 50000 ); // 50 ms
}
}
void sensorUpdater()
{
SensorManager sm;
int fixedValues[4];
int rateValues[4];
while( true )
{
sm.update();
sm.getValues( fixedValues );
sm.getRates( rateValues );
m.lock();
yaw = fixedValues[2];
pitch = fixedValues[1];
roll = fixedValues[0];
height = fixedValues[3];
yawRate = rateValues[2];
pitchRate = rateValues[1];
rollRate = rateValues[0];
heightRate = rateValues[3];
m.unlock();
}
}
void readUnLock() {
lock_guard<mutex> readerCountLock(readerCountLock_);
readerCount_--;
if (readerCount_ == 0) {
resourceLock_.unlock();
}
}
void STAGE_TWO() //HANDSHAKE STUFF
{
_mtx.lock() ;
flag-- ;
cout << " NUMBER OF THREADS WORKING: " << flag << endl ;
_mtx.unlock() ;
}
void ThreadMgr::Print(
const string& fname,
const string& tag) const
{
mtxPrint.lock();
ofstream fo;
fo.open(fname, std::ios_base::app);
fo << tag <<
": Real threads occupied (out of " << numRealThreads << "):\n";
for (unsigned t = 0; t < numRealThreads; t++)
{
if (realThreads[t])
fo << t << endl;
}
fo << endl;
fo << "Machine threads overview:\n";
for (unsigned t = 0; t < numMachineThreads; t++)
{
if (machineThreads[t] != -1)
{
fo << setw(4) << left << t << machineThreads[t] << endl;
}
}
fo << endl;
fo.close();
mtxPrint.unlock();
}
bool ThreadMgr::Release(const int machineId)
{
mtx.lock();
bool ret;
const unsigned m = static_cast<unsigned>(machineId);
const int r = machineThreads[m];
const unsigned ru = static_cast<unsigned>(r);
if (r == -1)
{
// Error: Not in use.
ret = false;
}
else if (! realThreads[ru])
{
// Error: Refers to a real thread that is not in use.
ret = false;
}
else
{
realThreads[ru] = false;
machineThreads[m] = -1;
ret = true;
}
mtx.unlock();
return ret;
}
void commander() //to determin whether the program can stop, and chane the omega used in SOR method
{
printf("thread Commander begins\n");
int i, CountAccept = 0;
double MaxError = 0;
// omega = 2/(1+Pi/Nx);
double BoundE = 0;
bool IsBoundMod = 1;
int WAIT = 20;
while(1)
{
sleep(WAIT);
MaxError = 0;
for(i = 0; i < ThreadNum; i++)
{
if(cseg[i].err > MaxError)
MaxError = cseg[i].err;
}
m.lock();
for(i = 0; i < ThreadNum; i++)
cseg[i].err = 0;
m.unlock();
// if(c%8==0)
printf("err: %e %e\n",MaxError, omega);
if(MaxError < MAXERR)
{
// BoundE = ModifyBound(Point);
BoundE = 0;
if(BoundE < BOUNDERR)
{
CountAccept++;
printf("CountAccept %d; BE %e\n", CountAccept, BoundE);
}
else
{
printf("BE %e\n", BoundE);
}
if(CountAccept == 1)
{
MAXERR = MAXERR2; //smaller error
WAIT = 20;
}
if(CountAccept > 5)
{
omega = 1;
break;
}
continue;
}
CountAccept = 0;
}
printf("WAIT ANOTHER 60 SECONDS TO TERMINATE\n");
sleep(60);
run = 0;
}
void sequence_read(int id, int size, int loop, char* fileName){
char* mem = (char *) malloc(size);
FILE* pFile;
char alterName[80];
strcpy(alterName, fileName);
alterName[strlen(fileName)] = id + '0';
alterName[strlen(fileName) + 1] = 0;
// cout << alterName << endl;
pFile = fopen(alterName, "w");
if(pFile == NULL) {
cout << "File error." << endl;
return;
}
int minSize = (int)pow(2.0, 30.0);
int readLoop = size * loop < minSize ? minSize / size : loop;
for (int i = 0; i < readLoop; ++i) {
fwrite(mem, sizeof(char), size, pFile);
}
fflush(pFile);
fclose(pFile);
pFile = fopen(alterName, "r");
if (pFile == NULL) {
cout << "File error." << endl;
return;
}
// get the size of file
fseek(pFile, 0, SEEK_END);
long fileSize = ftell(pFile);
rewind(pFile);
if (loop * size > fileSize) {
cout << "The file is only " << fileSize << "B, try a loop number smaller than " << fileSize / size << endl;
return;
}
struct timeval start, end;
gettimeofday(&start, NULL);
for (int i = 0; i < loop; ++i) {
fread(mem, sizeof(char), size, pFile);
// cout << mem << endl;
}
gettimeofday(&end, NULL);
double seconds = end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1000000.0;
if (remove(alterName) != 0) {
cout << "Error deleting file " << alterName << endl;
}
mu.lock();
durations.push_back(seconds);
mu.unlock();
fclose(pFile);
free(mem);
}
void forJoinThread(){ // Will run only in the introducer
int listenFd = open_socket(port + 1); //use the port next to UDP as TCP port
while(true)
{
int ret;
int connFd = listen_socket(listenFd);
logFile<<"ForJoinThread: one node asking for membership list"<<endl;
Message income;
char addr[4];
read(connFd, &income, sizeof(income));
//if this is normal node:
//if carrier is introducer, send back local list, then add carrier node
//else, just add carrier node
//if this is introducer node:
//send back local list. add carrier node. tell everyone carrier node is joining.
//me is a normal node
if(!isIntroducer){
//if introducer is joining
if(income.TTL == 1){
//give introducer my local list
sendBackLocalList(connFd);
}
//no matter what is the joining node, join it
addMember(income.carrierAdd, income.timeStamp);
}
else{ //this node is an introducer
//send back local list
sendBackLocalList(connFd);
//add carrier node
addMember(income.carrierAdd, income.timeStamp);
usleep( 10*1000 ); //wait to make sure last joined member has enough time to open TCP listening port
//tell everyone carrier node is joining
membersLock.lock();
thread ** broadThreads = new thread*[members.size() ]; //members[0] is introducer itself, members[size-1] is the node just joined
for(int i=1; i < members.size() - 1; i++)
broadThreads[i] = new thread(broadcastJoin, income, i);
for(int i=1; i < members.size() - 1; i++){
broadThreads[i]->join();
delete broadThreads[i];
}
delete [] broadThreads;
membersLock.unlock();
}
close(connFd);
//printMember();
}
return;
}
bool counter::end_of_data() {
// Should be const, but then couldn't lock it.
lock.lock();
bool end = producer_started and producer_count == 0
and data_count == 0;
lock.unlock();
return end;
}
void swap(queue_type &q) {
m_mutex.lock();
q.swap(m_queue);
if (m_queue.empty() && sleeping_on_empty) {
m_empty_conditional.signal();
}
m_mutex.unlock();
}
void ProduceData(T data)
{
m_mtx.lock();
m_PrintQueue.push(data);
m_mtx.unlock();
}
VideoDecode* VideoTextureCache::addVideo(const char *path)
{
VideoDecode* pVideoDecode = (VideoDecode*)m_pVideoDecodes->at(path);
pVideoDecode = new VideoDecode();
if(pVideoDecode->init(path)) {
m_pVideoDecodes->insert(path, pVideoDecode);
_threadEnd = false;
std::thread thread = std::thread([this](void *data){
VideoDecode *p = (VideoDecode *) data;
if(p) {
while(!_threadEnd && p->decode()) {
//sleep ?
if(_threadEnd)
break;
mtx.lock();
int size = (int)m_pTextures->size();
mtx.unlock();
while (size > 30) {
mtx.lock();
size = (int)m_pTextures->size();
mtx.unlock();
}
}
}
CC_SAFE_RELEASE_NULL(p);
},pVideoDecode);
thread.detach();
pVideoDecode->release();
s_pAsyncVideoPicQueue = new queue<VideoPic*>();
Director::getInstance()->getScheduler()->schedule(schedule_selector(VideoTextureCache::picToTexture), this, 0, false);
} else {
CCLOGERROR("CCVideoDecode init error in CCVideoTextureCache");
return NULL;
}
pVideoDecode->retain();
return pVideoDecode;
}
void sendAQFlightCommand(int aqfd) {
AQFDLock.lock();
tcflush(aqfd, TCIOFLUSH);
sendtoAQ32(aqfd, AQ32SendBuffer, AQ32SendBufferLen);
memset(AQ32SendBuffer, '\0', AQ_SENDBUF_SIZE);
AQ32SendBufferLen = 0;
AQFDLock.unlock();
}
inline void post() const {
mut.lock();
if (waitercount > 0) {
cond.signal();
}
semvalue++;
mut.unlock();
}
void take_forks(int i) {
m.lock();
state[i] = HUNGRY; // record fact that philosopher i is hungry
test(i); // try to aquire 2 forks
m.unlock();
lock[i].lock(); // block if forks were not acquired
}
void print (const string& name, size_t id, const buf_data& data,
const counter* count) {
lock.lock();
cout << timer.elapsed() << " " << name << " " << id
<< " " << to_string (data) << " ... " << to_string (*count)
<< endl << flush;
lock.unlock();
}
vid_t getNewIdRight(){
vid_t tmp = 0;
lock.lock();
tmp = right++;
lock.unlock();
assert(tmp < right_bound);
return tmp;
}
void XaLibLog::WriteFile (string MyLogString,string LogMessageLevel){
if (SETTINGS["LogLevel"]=="1"){
m.lock();
*MY_LOG_FILE<<MyLogString<<endl;
m.unlock();
} else if (SETTINGS["LogLevel"]=="2" && LogMessageLevel=="ERR"){
m.lock();
*MY_LOG_FILE<<MyLogString<<endl;
m.unlock();
}
};
void fun2() {
while(!finish) {
m.lock();
auto copy = ++counter;
cout << "fun2:" << counter << " : " << copy << endl;
m.unlock();
}
}
vid_t getNewIdLeft(){
vid_t tmp = 0;
lk.lock();
tmp = left++;
lk.unlock();
assert(tmp < left_bound);
return tmp;
}
trace_count::~trace_count() {
#ifdef USE_TRACEPOINT
printlock.lock();
print(std::cout, estimate_ticks_per_second());
std::cout.flush();
printlock.unlock();
#endif
}
void onError(shared_ptr<TcpConn> conn)
{
DEBUG("have client close connect or network error!");
g_connMtx.lock();
g_connlist.erase(conn->getSocket());
g_connMtx.unlock();
return;
}
/**
* Returns an element if the queue has an entry.
* returns [item, false] otherwise.
*/
inline std::pair<T, bool> try_dequeue() {
if (m_queue.empty() || m_alive == false) return std::make_pair(T(), false);
m_mutex.lock();
T elem = T();
// Wait while the queue is empty and this queue is alive
if (m_queue.empty() || m_alive == false) {
m_mutex.unlock();
return std::make_pair(elem, false);
}
else {
elem = m_queue.front();
m_queue.pop_front();
}
m_mutex.unlock();
return std::make_pair(elem, true);
}
void onConn(shared_ptr<TcpConn> conn)
{
DEBUG("have new client connect server!");
g_connMtx.lock();
g_connlist.insert(pair<int,shared_ptr<TcpConn>>(conn->getSocket(),conn));
g_connMtx.unlock();
return;
}
void ComponeApp::touchesBegan(cinder::app::TouchEvent event) {
_touchesMtx.lock();
for(auto &touch : event.getTouches()) {
auto nt = touch;
_touches[touch.getId()] = touch;
}
_touchesMtx.unlock();
}
void ComponeApp::touchesMoved(cinder::app::TouchEvent event) {
_pan += vec2(-1,1)*(event.getTouches()[0].getPos() - event.getTouches()[0].getPrevPos());
_touchesMtx.lock();
for(auto &touch : event.getTouches()) {
_touches[touch.getId()] = touch;
}
_touchesMtx.unlock();
}
void RaiseAnException(shared_ptr<boost::asio::io_service> io_service) {
global_lock5a.lock();
cout << "[" << std::this_thread::get_id() << "] " << __FUNCTION__ << endl;
global_lock5a.unlock();
io_service->post(bind(&RaiseAnException, io_service));
throw std::runtime_error("Oops!");
}
PDU& PacketSenderGeneric::send_recv(PDU& spdu, SharedSender& shared_sender, const NetworkInterface& iface, bool promisc, double* rdelay, double* edelay)
{
//wait for previous packet to receive response (TODO: not ideal, plan future change)
while (sent_pdu) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
sent_pdu = spdu.clone();
//start sniff task
SnifferConfiguration config;
config.set_promisc_mode(promisc);
config.set_snap_len(65535);
config.set_timeout(10);
//Critical section
SHARED_SNIFFER_MUTEX.lock();
Sniffer sniffer{ iface.name(), config };
SHARED_SNIFFER_MUTEX.unlock();
bool compute_delay = true;
if (!rdelay)
compute_delay = false;
std::future<void> fresp(std::async(std::launch::async, &PacketSenderGeneric::sniff_task, this, &sniffer, compute_delay));
//send packet
std::clock_t effective_sent_time = std::clock();
std::cout << "Registering packet to send !" << std::endl;
shared_sender.register_packet(sent_pdu, NetworkInterface(iface));
//std::cout << "waiting for max " << timeout << "..." << std::endl;
std::future_status status = fresp.wait_for(std::chrono::seconds(timeout));
//raise exception in case of timeout
if (status == std::future_status::timeout)
{
sniffer.stop_sniff();
sent_pdu = NULL;
throw timeout_elapsed();
}
else if (status == std::future_status::deferred)
std::cout << "DEBUG: packet sniffing deffered... shouldn't happen";
//Treat response packet
if (edelay)
*edelay = ((std::clock() - effective_sent_time) / (double)CLOCKS_PER_SEC) * 1000;
if (rdelay) {
*rdelay = response_delay;
}
PDU& response(*this->response_pdu);
//Clean
sent_pdu = NULL;
response_delay = NULL;
//response_pdu = NULL;
return response;
}
virtual void Process(float* out, uint32 numSamples) override
{
if(!m_playing)
return;
m_lock.lock();
if(m_format.nChannels == 2)
{
// Mix stereo sample
for(uint32 i = 0; i < numSamples; i++)
{
if(m_playbackPointer >= m_length)
{
// Playback ended
m_playing = false;
break;
}
int16* src = ((int16*)m_pcm.data()) + m_playbackPointer;
out[i * 2] = (float)src[0] / (float)0x7FFF;
out[i * 2 + 1] = (float)src[1] / (float)0x7FFF;
// Increment source sample with resampling
m_sampleStep += m_sampleStepIncrement;
while(m_sampleStep >= fp_sampleStep)
{
m_playbackPointer += 2;
m_sampleStep -= fp_sampleStep;
}
}
}
else
{
// Mix mono sample
for(uint32 i = 0; i < numSamples; i++)
{
if(m_playbackPointer >= m_length)
{
// Playback ended
m_playing = false;
break;
}
int16* src = ((int16*)m_pcm.data()) + m_playbackPointer;
out[i * 2] = (float)src[0] / (float)0x7FFF;
out[i * 2 + 1] = (float)src[0] / (float)0x7FFF;
// Increment source sample with resampling
m_sampleStep += m_sampleStepIncrement;
while(m_sampleStep >= fp_sampleStep)
{
m_playbackPointer += 1;
m_sampleStep -= fp_sampleStep;
}
}
}
m_lock.unlock();
}
