auto get_desire_quat() {
float heading, pitch, roll;
shm_get(desires, heading, heading);
shm_get(desires, pitch, pitch);
shm_get(desires, roll, roll);
return quat_from_hpr(heading, pitch, roll);
}
int shmget(key_t key,size_t size,int flags) {
int ret;
if ((flags&IPC_CREAT) || key==IPC_PRIVATE) ret = shm_create(key,size,flags);
if ((!(flags&IPC_CREAT) || ret<0) && !(flags&IPC_EXCL)) ret = shm_get(key);
errno = ret<0?-ret:0;
return ret<0?-1:ret;
}
TSRM_API int shmctl(int key, int cmd, struct shmid_ds *buf) {
shm_pair *shm = shm_get(key, NULL);
if (!shm->segment) {
return -1;
}
switch (cmd) {
case IPC_STAT:
memcpy(buf, shm->descriptor, sizeof(struct shmid_ds));
return 0;
case IPC_SET:
shm->descriptor->shm_ctime = time(NULL);
shm->descriptor->shm_perm.uid = buf->shm_perm.uid;
shm->descriptor->shm_perm.gid = buf->shm_perm.gid;
shm->descriptor->shm_perm.mode = buf->shm_perm.mode;
return 0;
case IPC_RMID:
if (shm->descriptor->shm_nattch < 1) {
shm->descriptor->shm_perm.key = -1;
}
return 0;
default:
return -1;
}
}
/*
In order to create a new message queue, or access an existing queue, the shmget() system call is used.
SYSTEM CALL: shmget();
PROTOTYPE: int shmget ( key_t key, int size, int shmflg );
RETURNS: shared memory segment identifier on success
-1 on error: errno = EINVAL (Invalid segment size specified)
EEXIST (Segment exists, cannot create)
EIDRM (Segment is marked for deletion, or was removed)
ENOENT (Segment does not exist)
EACCES (Permission denied)
ENOMEM (Not enough memory to create segment)
NOTES:
This particular call should almost seem like old news at this point.
It is strikingly similar to the corresponding get calls for message queues and semaphore sets.
The first argument to shmget() is the key value (in our case returned by a call to ftok()).
This key value is then compared to existing key values that exist within the kernel for other shared memory segments.
At that point, the open or access operation is dependent upon the contents of the shmflg argument.
IPC_CREAT
Create the segment if it doesn't already exist in the kernel.
IPC_EXCL
When used with IPC_CREAT, fail if segment already exists.
If IPC_CREAT is used alone, shmget() either returns the segment identifier for a newly created segment,
or returns the identifier for a segment which exists with the same key value.
If IPC_EXCL is used along with IPC_CREAT, then either a new segment is created, or if the segment exists, the call fails with -1.
IPC_EXCL is useless by itself, but when combined with IPC_CREAT, it can be used as a facility to guarantee that no existing segment is opened for access.
Once again, an optional octal mode may be OR'd into the mask.
Let's create a wrapper function for locating or creating a shared memory segment :
int open_segment( key_t keyval, int segsize )
{
int shmid;
if((shmid = shmget( keyval, segsize, IPC_CREAT | 0660 )) == -1)
{
return(-1);
}
return(shmid);
}
Note the use of the explicit permissions of 0660.
This small function either returns a shared memory segment identifier (int), or -1 on error.
The key value and requested segment size (in bytes) are passed as arguments.
Once a process has a valid IPC identifier for a given segment, the next step is for the process to attach or map the segment into its own addressing space.
*/
TSRM_API int shmget ( key_t key, int size, int shmflg ) { //int shmget(int key, int size, int flags) {
shm_pair *shm;
char shm_segment[26], shm_info[29];
HANDLE shm_handle, info_handle;
BOOL created = FALSE;
DWORD errnum; //for DEBUG
if (size < 0) {
return -1;
}
sprintf(shm_segment, "TSRM_SHM_SEGMENT:%d", key);
sprintf(shm_info, "TSRM_SHM_DESCRIPTOR:%d", key);
shm_handle = OpenFileMapping(FILE_MAP_ALL_ACCESS, FALSE, shm_segment);
info_handle = OpenFileMapping(FILE_MAP_ALL_ACCESS, FALSE, shm_info);
if ((!shm_handle && !info_handle)) {
if (shmflg & IPC_CREAT) {
shm_handle = CreateFileMapping(INVALID_HANDLE_VALUE, NULL, PAGE_READWRITE, 0, size, shm_segment);
info_handle = CreateFileMapping(INVALID_HANDLE_VALUE, NULL, PAGE_READWRITE, 0, sizeof(shm->descriptor), shm_info);
created = TRUE;
}
if ((!shm_handle || !info_handle)) {
errnum = GetLastError();
return -1;
}
} else {
if (shmflg & IPC_EXCL) {
return -1;
}
}
if ((shm = shm_get(CREATE_NEW_IF_NOT_EXISTS, key, NULL)) == NULL) return -1;
shm->segment = shm_handle;
shm->info = info_handle;
shm->descriptor = MapViewOfFileEx(shm->info, FILE_MAP_ALL_ACCESS, 0, 0, 0, NULL);
if (created) {
shm->descriptor->shm_perm.key = key;
shm->descriptor->shm_segsz = size;
shm->descriptor->shm_ctime = time(NULL);
shm->descriptor->shm_cpid = getpid();
shm->descriptor->shm_perm.mode = shmflg;
shm->descriptor->shm_perm.cuid = shm->descriptor->shm_perm.cgid= 0;
shm->descriptor->shm_perm.gid = shm->descriptor->shm_perm.uid = 0;
shm->descriptor->shm_atime = shm->descriptor->shm_dtime = 0;
shm->descriptor->shm_lpid = shm->descriptor->shm_nattch = 0;
shm->descriptor->shm_perm.mode = shm->descriptor->shm_perm.seq = 0;
}
if (shm->descriptor->shm_perm.key != key || size > shm->descriptor->shm_segsz ) {
CloseHandle(shm->segment);
UnmapViewOfFile(shm->descriptor);
CloseHandle(shm->info);
return -1;
}
return key;
}
/*
*
* Function: initShareMemory
* Description: 系统管理共享内存分配
* Input:
* OutPut:
*
* Return:
* Other:
*
*/
int CommandServer::initShareMemory( void )
{
int iSize, iMaxSys, iMaxChildPerSys, iShmId;
apr_status_t tStatus;
iMaxSys = m_pstConfig->m_iMaxSys;
iMaxChildPerSys = m_pstConfig->m_iMaxChild;
if( iMaxSys > 128 )
{
iMaxSys = 128;
}
if( iMaxSys < 16 )
{
iMaxSys = 16;
}
if( iMaxChildPerSys > 256 )
{
iMaxChildPerSys = 256;
}
if( iMaxChildPerSys < 2 )
{
iMaxChildPerSys = 2;
}
iSize = sizeof( ProcessRegister ) * iMaxSys * iMaxChildPerSys;
LOG4C(( LOG_DEBUG, "计算出的进程管理共享内存大小 %d\n", iSize ));
m_pstProcess = (ProcessRegister*)shm_get( m_pstConfig->m_iPrcShmKey, iSize );
if( m_pstProcess == NULL )
{
LOG4C(( LOG_DEBUG, "创建共享内存(KEY=%d)失败", m_pstConfig->m_iPrcShmKey ));
return -1;
}
iSize = sizeof(SystemRegister) * m_pstConfig->m_iMaxSys;
LOG4C(( LOG_DEBUG, "计算出的系统管理共享内存大小 %d\n", iSize ));
m_pstSystem = (SystemRegister*)shm_get( m_pstConfig->m_iGrpShmKey, iSize );
if( m_pstSystem == NULL )
{
LOG4C(( LOG_DEBUG, "创建共享内存(KEY=%d)失败", m_pstConfig->m_iGrpShmKey ));
return -1;
}
return 0;
}
int
sys_shm_get(void)
{
int key, addr;
argint(0,&key);
argint(1,&addr);
return shm_get(key,(void**) addr);
}
TSRM_API int shmdt(const void *shmaddr)
{
shm_pair *shm = shm_get(0, (void*)shmaddr);
if (!shm->segment) {
return -1;
}
shm->descriptor->shm_dtime = time(NULL);
shm->descriptor->shm_lpid = getpid();
shm->descriptor->shm_nattch--;
return UnmapViewOfFile(shm->addr) ? 0 : -1;
}
TSRM_API void *shmat(int key, const void *shmaddr, int flags)
{
shm_pair *shm = shm_get(key, NULL);
if (!shm->segment) {
return (void*)-1;
}
shm->descriptor->shm_atime = time(NULL);
shm->descriptor->shm_lpid = getpid();
shm->descriptor->shm_nattch++;
shm->addr = MapViewOfFileEx(shm->segment, FILE_MAP_ALL_ACCESS, 0, 0, 0, NULL);
return shm->addr;
}
/*
SYSTEM CALL: shmat();
PROTOTYPE: int shmat ( int shmid, char *shmaddr, int shmflg);
RETURNS: address at which segment was attached to the process, or
-1 on error: errno = EINVAL (Invalid IPC ID value or attach address passed)
ENOMEM (Not enough memory to attach segment)
EACCES (Permission denied)
NOTES:
If the addr argument is zero (0), the kernel tries to find an unmapped region.
This is the recommended method.
An address can be specified, but is typically only used to facilitate proprietary hardware
or to resolve conflicts with other apps.
The SHM_RND flag can be OR'd into the flag argument to force a passed address to be page aligned (rounds down to the nearest page size).
In addition, if the SHM_RDONLY flag is OR'd in with the flag argument, then the shared memory segment will be mapped in, but marked as readonly.
This call is perhaps the simplest to use.
Consider this wrapper function, which is passed a valid IPC identifier for a segment, and returns the address that the segment was attached to:
char *attach_segment( int shmid )
{
return(shmat(shmid, 0, 0));
}
Once a segment has been properly attached, and a process has a pointer to the start of that segment,
reading and writing to the segment become as easy as simply referencing or dereferencing the pointer!
Be careful not to lose the value of the original pointer! If this happens, you will have no way of accessing the base (start) of the segment.
*/
TSRM_API void *shmat(int shmid, const void *shmaddr, int shmflg) { //void *shmat(int key, const void *shmaddr, int flags) {
shm_pair *shm;
if ((shm = shm_get(GET_IF_EXISTS_BY_KEY, shmid, NULL)) == NULL) return (void*)-1;
if (!shm->segment) {
return (void*)-1;
}
shm->descriptor->shm_atime = time(NULL);
shm->descriptor->shm_lpid = getpid();
shm->descriptor->shm_nattch++;
shm->addr = MapViewOfFileEx(shm->segment, FILE_MAP_ALL_ACCESS, 0, 0, 0, NULL);
return shm->addr;
}
/*
SYSTEM CALL: shmdt();
PROTOTYPE: int shmdt ( char *shmaddr );
RETURNS: -1 on error: errno = EINVAL (Invalid attach address passed)
After a shared memory segment is no longer needed by a process, it should be detached by calling this system call.
As mentioned earlier, this is not the same as removing the segment from the kernel!
After a detach is successful, the shm_nattch member of the associates shmid_ds structure is decremented by one.
When this value reaches zero (0), the kernel will physically remove the segment.
*/
TSRM_API int shmdt(const void *shmaddr) {
int ret;
shm_pair *shm;
if ((shm = shm_get(GET_IF_EXISTS_BY_ADDR, 0, (void*)shmaddr)) == NULL) return -1;
if (!shm->segment) {
return -1;
}
shm->descriptor->shm_dtime = time(NULL);
shm->descriptor->shm_lpid = getpid();
shm->descriptor->shm_nattch--;
//return UnmapViewOfFile(shm->addr) ? 0 : -1;
//nestaci, nakolko nemam ZEND garbage collector, musim explicitne sam uzavriet vsetko potrebne:
//(A JE VLASTNE JEDNO, ci som creator, alebo nie - system si to osetri sam (pokial mu NATVRDO nedam shm_nattch = 0; ?! ))
ret = UnmapViewOfFile(shm->addr);
CloseHandle(shm->segment);
UnmapViewOfFile(shm->descriptor);
CloseHandle(shm->info);
return ret;
}
TSRM_API int shmget(int key, int size, int flags)
{
shm_pair *shm;
char shm_segment[26], shm_info[29];
HANDLE shm_handle, info_handle;
BOOL created = FALSE;
if (size < 0) {
return -1;
}
snprintf(shm_segment, sizeof(shm_segment), "TSRM_SHM_SEGMENT:%d", key);
snprintf(shm_info, sizeof(shm_info), "TSRM_SHM_DESCRIPTOR:%d", key);
shm_handle = OpenFileMapping(FILE_MAP_ALL_ACCESS, FALSE, shm_segment);
info_handle = OpenFileMapping(FILE_MAP_ALL_ACCESS, FALSE, shm_info);
if (!shm_handle && !info_handle) {
if (flags & IPC_CREAT) {
shm_handle = CreateFileMapping(INVALID_HANDLE_VALUE, NULL, PAGE_READWRITE, 0, size, shm_segment);
info_handle = CreateFileMapping(INVALID_HANDLE_VALUE, NULL, PAGE_READWRITE, 0, sizeof(shm->descriptor), shm_info);
created = TRUE;
}
if (!shm_handle || !info_handle) {
if (shm_handle) {
CloseHandle(shm_handle);
}
if (info_handle) {
CloseHandle(info_handle);
}
return -1;
}
} else {
if (flags & IPC_EXCL) {
return -1;
}
}
shm = shm_get(key, NULL);
if (!shm) {
CloseHandle(shm_handle);
CloseHandle(info_handle);
return -1;
}
shm->segment = shm_handle;
shm->info = info_handle;
shm->descriptor = MapViewOfFileEx(shm->info, FILE_MAP_ALL_ACCESS, 0, 0, 0, NULL);
if (NULL != shm->descriptor && created) {
shm->descriptor->shm_perm.key = key;
shm->descriptor->shm_segsz = size;
shm->descriptor->shm_ctime = time(NULL);
shm->descriptor->shm_cpid = getpid();
shm->descriptor->shm_perm.mode = flags;
shm->descriptor->shm_perm.cuid = shm->descriptor->shm_perm.cgid= 0;
shm->descriptor->shm_perm.gid = shm->descriptor->shm_perm.uid = 0;
shm->descriptor->shm_atime = shm->descriptor->shm_dtime = 0;
shm->descriptor->shm_lpid = shm->descriptor->shm_nattch = 0;
shm->descriptor->shm_perm.mode = shm->descriptor->shm_perm.seq = 0;
}
if (NULL != shm->descriptor && (shm->descriptor->shm_perm.key != key || size > shm->descriptor->shm_segsz)) {
if (NULL != shm->segment) {
CloseHandle(shm->segment);
}
UnmapViewOfFile(shm->descriptor);
CloseHandle(shm->info);
return -1;
}
return key;
}
int main()
{
int bufferFilledSpaces, bufferEmptySpaces, semM, semN;
char *buffer;
int temp;
int shmid;
char c1, c2;
int input = 0;
int counter;
char *arg[1] = {0};
int N;
// Create duplicate process and give it code from ProcN
N = fork();
if (N == 0) // process is a child
{
execv("ProcN", arg);
}
// Create semaphore bufferFilledSpaces
bufferFilledSpaces = sem_create(155555, 0);
if (bufferFilledSpaces < 0)
{
Error(1); // Error has occurred
}
// Create semaphore bufferEmptySpaces
bufferEmptySpaces = sem_create(255555, 1);
if (bufferEmptySpaces < 0)
{
Error(1); // Error has occurred
}
// Creates synchronization semaphores
semM = sem_create(355555, 0);
if (semM < 0)
{
Error(1); // Error has occurred
}
// Creates synchronization semaphores
semN = sem_create(455555, 0);
if (semN < 0)
{
Error(1); // Error has occurred
}
// Opens shared memory between two processes
shmid = shm_get(123123, (void*) &buffer, 90);
// Synchronization with Process N
temp = sem_signal(semN);
temp = sem_wait(semM);
// Part 1 - Fill in initial values
c1 = 'a';
c2 = '0';
counter = 1;
while (counter <= 260)
{
temp = sem_wait(bufferEmptySpaces);
buffer[input] = c1;
buffer[input+1] = c2;
buffer[input+2] = c1;
//Debug
//printf("# %c%c%c #", c1, c2, c1);
if ((counter % 26) == 0) // Increment number if needed
{
c2++;
c1 = 'a';
}
else // Otherwise, move to next letter in alphabet
{
c1++;
}
counter++;
temp = sem_signal(bufferFilledSpaces);
}
// Part 2 - Fill in initial values
c1 = 'A';
c2 = '0';
counter = 1;
while (counter <= 260)
{
temp = sem_wait(bufferEmptySpaces);
buffer[input] = c1;
buffer[input+1] = c2;
buffer[input+2] = c1;
//Debug
//printf("# %c%c%c #", c1, c2, c1);
if ((counter % 26) == 0) // Increment number if needed
{
c2++;
c1 = 'A';
}
else // Otherwise, move to next letter in alphabet
{
c1++;
}
counter++;
temp = sem_signal(bufferFilledSpaces);
}
// Synchronization with Process N
temp = sem_signal(semN);
temp = sem_wait(semM);
// Remove semaphores
temp = sem_rm(bufferEmptySpaces);
temp = sem_rm(bufferFilledSpaces);
temp = sem_rm(semM);
temp = sem_rm(semN);
// Remove shared memory
temp = shm_rm(shmid);
printf("\n");
return 1;
}
int main(int argc, char **argv)
{
int option;
int i;
bool killall = false;
bool verbose = false;
int nsamples = INT_MAX;
time_t timeout = (time_t)0, starttime = time(NULL);
/* a copy of all old segments */
struct shm_stat_t shm_stat_old[NTPSEGMENTS + 1];;
memset( shm_stat_old, 0 ,sizeof( shm_stat_old));
while ((option = getopt(argc, argv, "hn:st:vV")) != -1) {
switch (option) {
case 'n':
nsamples = atoi(optarg);
break;
case 's':
killall = true;
break;
case 't':
timeout = (time_t)atoi(optarg);
break;
case 'v':
verbose = true;
break;
case 'V':
(void)fprintf(stderr, "%s: version %s (revision %s)\n",
argv[0], VERSION, REVISION);
exit(EXIT_SUCCESS);
case 'h':
fprintf(stderr,
"usage: ntpshmmon [-s] [-n max] [-t timeout] [-v] [-h] [-V]\n"
" -h print this help\n"
" -n nsamples exit after nsamples\n"
" -s remove SHMs and exit\n"
" -t nseconds exit after nseconds\n"
" -v be verbose\n"
" -V print version and exit\n"
);
exit(EXIT_SUCCESS);
default:
/* no option, just go and do it */
break;
}
}
/* grab all segments, keep the non-null ones */
for (i = 0; i < NTPSEGMENTS; i++) {
segments[i] = shm_get(i, false, true);
if (verbose && segments[i] != NULL)
fprintf(stderr, "unit %d opened\n", i);
}
if (killall) {
struct shmTime **pp;
for (pp = segments; pp < segments + NTPSEGMENTS; pp++)
if (*pp != NULL)
(void)shmdt((void *)(*pp));
exit(EXIT_SUCCESS);
}
/*
* We want line buffering even if stdout is going to a file. This
* is a (possibly futile) attempt to avoid writing an incomplete
* line on interrupt.
*/
setvbuf(stdout, NULL, _IOLBF, 0);
(void)printf("ntpshmmon version 1\n");
(void)printf("# Name Seen@ Clock Real L Prec\n");
do {
/* the current segment */
struct shm_stat_t shm_stat;
struct timespec delay;
for (i = 0; i < NTPSEGMENTS; i++) {
long long diff; /* 32 bit long is too short for a timespec */
enum segstat_t status = ntp_read(segments[i], &shm_stat, false);
if (verbose)
fprintf(stderr, "unit %d status %d\n", i, status);
switch(status) {
case OK:
/* ntpd can slew the clock at 120% real time
* so do not lock out slightly short cycles
* use 50% of cycle time as lock out limit.
* ignore that system time may jump. */
diff = timespec_diff_ns(shm_stat.tvr, shm_stat_old[i].tvr);
if ( 0 == diff) {
/* no change in tvr */
break;
}
diff = timespec_diff_ns(shm_stat.tvt, shm_stat_old[i].tvt);
if ( 0 == diff) {
/* no change in tvt */
break;
}
/* time stamp it */
clock_gettime(CLOCK_REALTIME, &shm_stat.tvc);
printf("sample %s %ld.%09ld %ld.%09ld %ld.%09ld %d %3d\n",
ntp_name(i),
(long)shm_stat.tvc.tv_sec, shm_stat.tvc.tv_nsec,
(long)shm_stat.tvr.tv_sec, shm_stat.tvr.tv_nsec,
(long)shm_stat.tvt.tv_sec, shm_stat.tvt.tv_nsec,
shm_stat.leap, shm_stat.precision);
--nsamples;
/* save the new time stamp */
shm_stat_old[i] = shm_stat; /* structure copy */
break;
case NO_SEGMENT:
break;
case NOT_READY:
/* do nothing, data not ready, wait another cycle */
break;
case BAD_MODE:
fprintf(stderr, "ntpshmmon: unknown mode %d on segment %s\n",
shm_stat.status, ntp_name(i));
break;
case CLASH:
/* do nothing, data is corrupt, wait another cycle */
break;
default:
fprintf(stderr, "ntpshmmon: unknown status %d on segment %s\n",
status, ntp_name(i));
break;
}
}
/* all segments now checked */
/*
* Even on a 1 Hz PPS, a sleep(1) may end up
* being sleep(1.1) and missing a beat. Since
* we're ignoring duplicates via timestamp, polling
* at fast intervals should not be a problem
*
* PPS is not always one pulse per second.
* the Garmin GPS 18x-5Hz outputs 5 pulses per second.
* That is a 200 millSec cycle, minimum 20 milliSec duration
* we will wait 1 milliSec out of caution
*
* and, of course, nanosleep() may sleep a lot longer than we ask...
*/
if ( timeout ) {
/* do not read time unless it matters */
if ( time(NULL) > (starttime + timeout ) ) {
/* time to exit */
break;
}
}
/* wait 1,000 uSec */
delay.tv_sec = 0;
delay.tv_nsec = 1000000L;
nanosleep(&delay, NULL);
} while ( 0 < nsamples );
exit(EXIT_SUCCESS);
}
int main()
{
int *BufferA;
int *BufferB;
char c1, c2, c3;
int bufAFilled, bufAEmpty, bufBFilled, bufBEmpty;
int semX, semY, semZ, CSX, CSY, CSZ;
int pX, pY;
char *arg[1] = {0};
int shmid1, shmid2;
int temp, loop_finished, counter, input;
// Create semaphore bufAFilled
bufAFilled = sem_create(500001, 0);
if (bufAFilled < 0)
{
Error(1); // Error has occurred
}
// Create semaphore bufAEmpty
bufAEmpty = sem_create(500002, 11);
if (bufAEmpty < 0)
{
Error(1); // Error has occurred
}
// Create semaphore bufBFilled
bufBFilled = sem_create(500003, 0);
if (bufBFilled < 0)
{
Error(1); // Error has occurred
}
// Create semaphore bufBEmpty
bufBEmpty = sem_create(500004, 23);
if (bufBEmpty < 0)
{
Error(1); // Error has occurred
}
// Create semaphore semX
semX = sem_create(500005, 0);
if (semX < 0)
{
Error(1); // Error has occurred
}
// Create semaphore semY
semY = sem_create(500006, 0);
if (semY < 0)
{
Error(1); // Error has occurred
}
// Create semaphore semZ
semZ = sem_create(500007, 0);
if (semZ < 0)
{
Error(1); // Error has occurred
}
// Create semaphore CSX
CSX = sem_create(500008, 0);
if (CSX < 0)
{
Error(1); // Error has occurred
}
// Create semaphore CSY
CSY = sem_create(500009, 0);
if (CSY < 0)
{
Error(1); // Error has occurred
}
// Create semaphore CSZ
CSZ = sem_create(500010, 0);
if (CSZ < 0)
{
Error(1); // Error has occurred
}
// Opens shared memory between two processes
shmid1 = shm_get(900001, (void*) &BufferA, 22);
// Opens shared memory between two processes
shmid2 = shm_get(900002, (void*) &BufferB, 69);
// Create duplicate processes and give them code for processes X and Y
pX = fork();
if (pX == 0) // process is a child
{
execv("ProcX", arg);
}
pY = fork();
if (pY == 0) // process is a child
{
execv("ProcY", arg);
}
// Synchronize with ProcX and ProcY
temp = sem_signal(semX);
temp = sem_signal(semY);
temp = sem_wait(semZ);
temp = sem_wait(semZ);
// Take out items from buffers when elements exist
counter = 1;
input = 0;
while (counter <= 260)
{
// Wait until both of the buffers have been filled
temp = sem_wait(CSZ);
temp = sem_wait(CSZ);
// Remove one item from BufferA when one exists
temp = sem_wait(bufAFilled);
c1 = BufferA[input*2];
c2 = BufferA[input*2+1];
printf("+ %c%c +", c1, c2);
temp = sem_signal(bufAEmpty);
// Remove one item from BufferB when one exists
temp = sem_wait(bufBFilled);
c1 = BufferB[input*3];
c2 = BufferB[input*3+1];
c3 = BufferB[input*3+2];
printf("+ %c%c%c +", c1, c2, c3);
temp = sem_signal(bufBEmpty);
if ((counter % 160) == 0) // Sleep after taking every 160 items
{
// printf("-!Sleeping 3 seconds!-");
// sleep(3);
}
counter++;
// Signal the commencement of the filling both the buffers
temp = sem_signal(CSX);
temp = sem_signal(CSY);
}
// Synchronize with ProcX and ProcY
temp = sem_signal(semX);
temp = sem_signal(semY);
temp = sem_wait(semZ);
temp = sem_wait(semZ);
// sleep(7);
printf("\n");
// Remove semaphores
temp = sem_rm(bufAEmpty);
temp = sem_rm(bufAFilled);
temp = sem_rm(bufBEmpty);
temp = sem_rm(bufBFilled);
temp = sem_rm(semX);
temp = sem_rm(semY);
temp = sem_rm(semZ);
temp = sem_rm(CSX);
temp = sem_rm(CSY);
temp = sem_rm(CSZ);
// Remove shared memory
temp = shm_rm(shmid1);
temp = shm_rm(shmid2);
printf("\nProcZ: Exiting...\n");
return 1;
}
void des_thrusters::step(double delta)
{
bool softkilled;
shm_get(switches, soft_kill, softkilled);
shm_getg(settings_control, shm_settings);
if (softkilled || !shm_settings.enabled) {
f -= f;
t -= t;
return;
}
// Read PID settings & desires.
// (would switching to watchers improve performance?)
SHM_GET_PID(settings_heading, shm_hp, hp)
SHM_GET_PID(settings_pitch, shm_pp, pp)
SHM_GET_PID(settings_roll, shm_rp, rp)
SHM_GET_PID(settings_velx, shm_xp, xp)
SHM_GET_PID(settings_vely, shm_yp, yp)
SHM_GET_PID(settings_depth, shm_dp, dp)
shm_getg(desires, shm_desires);
// Read current orientation, velocity & position (in the model frame).
// Orientation quaternion in the model frame.
Eigen::Quaterniond qm = q * mtob_rq;
Eigen::Vector3d rph = quat_to_euler(qm);
// Component of the model quaternion about the z-axis (heading-only rotation).
Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0);
// Velocity in heading modified world space.
Eigen::Vector3d vp = qh.conjugate() * v;
// Step the PID loops.
Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD,
shm_desires.pitch * DEG_TO_RAD,
shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG;
double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG);
double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG);
double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG);
double xo = xp.step(delta, shm_desires.speed, vp[0]);
double yo = yp.step(delta, shm_desires.sway_speed, vp[1]);
double zo = dp.step(delta, shm_desires.depth, x[2]);
// f is in the heading modified world frame.
// t is in the model frame.
// We will work with f and b in the model frame until the end of this
// function.
f[0] = shm_settings.velx_active ? xo : 0;
f[1] = shm_settings.vely_active ? yo : 0;
f[2] = shm_settings.depth_active ? zo : 0;
f *= m;
Eigen::Vector3d w_in;
w_in[0] = shm_settings.roll_active ? ro : 0;
w_in[1] = shm_settings.pitch_active ? po : 0;
w_in[2] = shm_settings.heading_active ? ho : 0;
// TODO Avoid this roundabout conversion from hpr frame
// to world frame and back to model frame.
Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0);
Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) +
qh * Eigen::Vector3d(0, w_in[1], 0) +
qhp * Eigen::Vector3d(w_in[0], 0, 0));
t = btom_rm * q.conjugate() * I * q * mtob_rm * w;
// Output diagnostic information. Shown by auv-control-helm.
SHM_PUT_PID(control_internal_heading, ho)
SHM_PUT_PID(control_internal_pitch, po)
SHM_PUT_PID(control_internal_roll, ro)
SHM_PUT_PID(control_internal_velx, xo)
SHM_PUT_PID(control_internal_vely, yo)
SHM_PUT_PID(control_internal_depth, zo)
// Subtract passive forces.
// (this implementation does not support discrimination!)
if (shm_settings.buoyancy_forces || shm_settings.drag_forces) {
// pff is in the body frame.
screw ps = pff();
f -= qh.conjugate() * q * ps.first;
t -= btom_rm * ps.second;
}
// Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of
// energy the sub can output (e.g. can't move forward at full speed
// and pitch at full speed at the same time).
// Doesn't really account for real-world thruster configurations (e.g.
// it might be possible to move forward at full speed and ascend at
// full speed at the same time) but it's just an approximation.
for (int i = 0; i < 3; i++) {
f[i] /= axes[i];
}
for (int i = 0; i < 3; i++) {
t[i] /= axes[3 + i];
}
double m = f.dot(f) + t.dot(t);
if (m > 1) {
double sm = sqrt(m);
f /= sm;
t /= sm;
}
for (int i = 0; i < 3; i++) {
f[i] *= axes[i];
}
for (int i = 0; i < 3; i++) {
t[i] *= axes[3 + i];
}
// Regular min/max clamping.
clamp(f, fa, fb, 3);
clamp(t, ta, tb, 3);
f = q.conjugate() * qh * f;
t = mtob_rm * t;
}
void Queue::queue_read_init(){
int shmid = shm_get();
_shmid = shmid;
_head = (Head *)(shm_attach(shmid));
_arr = (Text *)(_head+1);
}
int main()
{
char *BufferB;
char c1, c2;
int bufBFilled, bufBEmpty;
int semX, semY, semZ, CSX, CSY, CSZ;
int shmid2;
int temp;
int counter, output;
// Open semaphore bufBFilled
bufBFilled = sem_open(500003);
if (bufBFilled < 0)
{
Error(1); // Error has occurred
}
// Open semaphore bufBEmpty
bufBEmpty = sem_open(500004);
if (bufBEmpty < 0)
{
Error(2); // Error has occurred
}
// Open semaphore semX
semX = sem_open(500005);
if (semX < 0)
{
Error(3); // Error has occurred
}
// Open semaphore semY
semY = sem_open(500006);
if (semY < 0)
{
Error(4); // Error has occurred
}
// Open semaphore semZ
semZ = sem_open(500007);
if (semZ < 0)
{
Error(5); // Error has occurred
}
// Open semaphore CSX
CSX = sem_open(500008);
if (CSX < 0)
{
Error(1); // Error has occurred
}
// Open semaphore CSY
CSY = sem_open(500009);
if (CSY < 0)
{
Error(1); // Error has occurred
}
// Open semaphore CSZ
CSZ = sem_open(500010);
if (CSZ < 0)
{
Error(1); // Error has occurred
}
// Opens shared memory between two processes
shmid2 = shm_get(900002, (void*) &BufferB, 69);
// Initialize characters for insertion into shared memory buffer
c1 = 'A';
c2 = '0';
counter = 1;
output = 0;
// Synchronize with ProcX and ProcZ
temp = sem_signal(semX);
temp = sem_signal(semZ);
temp = sem_wait(semY);
temp = sem_wait(semY);
// Insert items into buffer when there is room
while (counter <= 260)
{
temp = sem_wait(bufBEmpty);
BufferB[output*3] = c1;
BufferB[output*3+1] = c1;
BufferB[output*3+2] = c2;
printf(" %c%c%c ", BufferB[output*3], BufferB[output*3+1], BufferB[output*3+2]);
temp = sem_signal(bufBFilled);
if ((counter % 26) == 0) // Increment number if needed
{
c2++;
c1 = 'A';
}
else // Otherwise, move to next letter in alphabet
{
c1++;
}
if ((counter % 100) == 0) // Sleep after every 100 items
{
// printf("-!Sleeping 2 seconds!-");
// sleep(2);
}
counter++;
// Signal the consumer to take an element
temp = sem_signal(CSZ);
temp = sem_wait(CSY);
}
// Synchronize with ProcX and ProcZ
temp = sem_signal(semX);
temp = sem_signal(semZ);
temp = sem_wait(semY);
temp = sem_wait(semY);
printf("\nProcY: Exiting...\n");
return 1;
}
