float readSteadyHeading() {
float f_ypr[3];
float heading = 0;
for (uint8_t i=0; i<MPU_LOOPS;) {
i += readHeading(f_ypr);
heading += f_ypr[0];
delay(MPU_PAUSES);
}
heading /= ((float)MPU_LOOPS);
heading = toCircle(-heading + (DEVICE_ORIENTATION * PI));
current_pitch = f_ypr[1] * 180.0 / PI;
current_roll = f_ypr[2] * 180.0 / PI;
// heading = (-heading) - (PI / 2.0);
logln("AHRS (y,p,r): %d, %d, %d",
((int16_t) (heading * 180.0 / PI)),
((int16_t) (current_pitch)),
((int16_t) (current_roll))
);
return heading;
}
bool Skeleton::readASF(char* filename) {
FILE* file = fopen(filename, "r");
if (file == NULL) {
printf("Failed to open file %s\n", filename);
exit(EXIT_FAILURE);
}
char* buff = new char[buffSize];
char *p;
printf("Starting reading skeleton file\n");
while ((p = fgets(buff, buffSize, file)) != NULL) {
//Reading actually happened!
char* start = buff;
trim(&start);
//Check if it is a comment or just empty
if (buff[0] == '#' || buff[0] == '\0') {
continue;
}
start = strchr(buff, ':');
if (start != NULL) {
//This actually is a heading
//Reading may actually consume the next heading
//so headings need to be a recursive construct?
readHeading(buff, file);
}
}
delete[] buff;
fclose(file);
printf("Completed reading skeleton file\n");
return true;
}
void Skeleton::readHeading(char* buff, FILE* file) {
char* temp = buff;
decomment(buff);
trim(&temp);
char head[50];
char rest[200];
int num = sscanf(temp, ":%s %s", head, rest);
if (num == 0) {
printf("Could not get heading name from %s, all is lost\n", temp);
exit(EXIT_FAILURE);
}
if (strcmp(head, "version") == 0) {
//version string - must be 1.10
char* version = rest;
if (num != 2) {
char *p = { '\0' };
while (strlen(p) == 0) {
char* p = fgets(buff, buffSize, file);
decomment(p);
trim(&p);
version = p;
}
}
if (strcmp(version, "1.10") != 0) {
printf("Invalid version: %s, must be 1.10\n", version);
exit(EXIT_FAILURE);
}
//Finished reading version so read the next thing?
} else if (strcmp(head, "name") == 0) {
//This allows the skeleton to be called something
//other than the file name
//We don't actually care what the name is, so can
//ignore this whole section
} else if (strcmp(head, "documentation") == 0) {
//Documentation section has no meaningful information
//only of use if you want to copy the file. So we skip it
} else if (strcmp(head, "units") == 0) {
//Has factors for the units
//To be able to model the real person,
//these must be parsed correctly
//Only really need to check if deg or rad, but even
//that is probably not needed for the core or extension
} else if (strcmp(head, "root") == 0) {
//Read in information about root
//Or be lazy and just assume it is going to be the normal CMU thing!
} else if (strcmp(head, "bonedata") == 0) {
//Description of each bone
//This does need to actually be read :(
char *p;
while ((p = fgets(buff, buffSize, file)) != NULL) {
decomment(p);
trim(&p);
if (strlen(p) > 0) {
if (p[0] == ':') {
return readHeading(buff, file);
} else if (strcmp(p, "begin") != 0) {
printf("Expected begin for bone data %d, found \"%s\"", numBones, p);
exit(EXIT_FAILURE);
} else {
readBone(buff, file);
numBones++;
}
}
}
} else if (strcmp(head, "hierarchy") == 0) {
//Description of how the bones fit together
char *p;
while ((p = fgets(buff, buffSize, file)) != NULL) {
trim(&p);
decomment(p);
if (strlen(p) > 0) {
if (p[0] == ':') {
return readHeading(buff, file);
} else if (strcmp(p, "begin") != 0) {
printf("Expected begin in hierarchy, found %s", p);
exit(EXIT_FAILURE);
} else {
readHierarchy(buff, file);
}
}
}
} else {
printf("Unknown heading %s\n", head);
}
}
void writeCalibrationLine() {
float f_ypr[3];
readHeading(f_ypr);
}
