/*
* Yaw, pitch, roll are in degree in input
*/
int post_data( POSTER_ID id, double x, double y, double z,
double yaw, double pitch, double roll)
{
// Variables to use for writing the poster
int offset = 0;
// try to get the pom reference frame
// if we can't get it, just returns
if (ref_id == NULL) {
if (poster_not_found_message == 1)
fprintf(stderr, "ref id is NULL : searching for %s\n", ref_name);
if (posterFind(ref_name, &ref_id) == ERROR) {
if (poster_not_found_message == 1){
fprintf(stderr, "can't find %s : looping\n", ref_name);
poster_not_found_message = 0;
}
ref_id = NULL;
return -1;
}
if (poster_not_found_message == 0){
// poster found! re-enable poster_not_found_message in case we loose it
// (is it actually possible?)
fprintf(stderr, "Found POM poster %s. Good.\n", ref_name);
poster_not_found_message = 1;
}
}
// Declare local versions of the structures used
POM_SENSOR_POS framePos;
posterRead(ref_id, 0, &framePos, sizeof(POM_SENSOR_POS));
POM_ME_POS* pos = posterAddr(id);
posterTake(id, POSTER_WRITE);
// Fill in the POM_POS_EULER
// yaw, pitch, roll are expected in radian
pos->main.euler.yaw = yaw;
pos->main.euler.pitch = pitch;
pos->main.euler.roll = roll;
pos->main.euler.x = x;
pos->main.euler.y = y;
pos->main.euler.z = z;
pos->date1 = framePos.date;
posterGive(id);
return 0;
}
int main(int argc, char *argv[])
{
POSTER_ID posterIdC;
POSTER_ID posterIdF;
TESTPOST_STR testIn = INIT_TESTPOST_STR;
TESTPOST_STR testOut;
int size = sizeof(TESTPOST_STR);
int sizeOut;
H2_ENDIANNESS endianness;
char c;
char msg[256];
printf ("testPosterWrite: LOCAL ENDIANNESS = %d\n", h2localEndianness());
/* create */
if (posterCreate(TEST_POSTER_NAME, size, &posterIdC) == ERROR) {
printf ("testPosterWrite: posterCreate(%s) failed : %s\n",
TEST_POSTER_NAME, h2getErrMsg(errnoGet(), msg, 256));
return 1;
}
printf ("testPosterWrite: poster %s created. Id = %0x\n",
TEST_POSTER_NAME, (int)posterIdC);
posterEndianness(posterIdC, &endianness);
printf ("testPosterWrite: posterEndianness(%s) = %d\n\n",
TEST_POSTER_NAME, endianness);
/* write */
if ((sizeOut=posterWrite(posterIdC, 0, (void *)&testIn, size)) != size) {
printf ("testPosterWrite: posterWrite size = %d != %d \n", sizeOut, size);
return testEnd(posterIdC);
}
printf ("testPosterWrite: poster %s writen\n", TEST_POSTER_NAME);
/* read after create */
if ((sizeOut=posterRead(posterIdC, 0, (void *)&testOut, size)) != size) {
printf ("testPosterWrite: posterRead size = %d != %d \n", sizeOut, size);
return testEnd(posterIdC);
}
printf ("testPosterWrite: poster %s read OK\n", TEST_POSTER_NAME);
/* display */
if (endianness != h2localEndianness())
endianswap_TESTPOST_STR(&testOut);
printTestStr("Write", &testOut);
printf ("\n");
/* find */
if (posterFind(TEST_POSTER_NAME, &posterIdF) == ERROR) {
printf ("testPosterWrite: posterFind failed\n");
return testEnd(posterIdC);
}
/* printf ("posterFind TEST_POSTER_NAME posterIdF = %d\n", (int)posterIdF); */
posterEndianness(posterIdF, &endianness);
printf (" endianness = %d\n", endianness);
/* read after find */
if ((sizeOut=posterRead(posterIdF, 0, (void *)&testOut, size)) != size) {
printf ("testPosterWrite: posterRead size = %d != %d \n", sizeOut, size);
return testEnd(posterIdC);
}
/* display */
if (endianness != h2localEndianness())
endianswap_TESTPOST_STR(&testOut);
printTestStr("Write", &testOut);
/* find again */
if (posterFind(TEST_POSTER_NAME, &posterIdF) == ERROR) {
printf ("testPosterWrite: posterFind failed\n");
return testEnd(posterIdC);
}
printf ("PosterIdF %s = %d\n", TEST_POSTER_NAME, (int)posterIdF);
posterEndianness(posterIdF, &endianness);
printf (" endianness = %d\n", endianness);
printf ("Return to end ....");
fflush(stdout);
scanf("%c", &c);
/* end */
return testEnd(posterIdF);
}
