void genCode()
{
printProc("Generating code... ");
printStatus("DONE",10);
printProc("Writing on file... ");
printStatus("DONE",10);
writeHeader();
int i, k;
for(i=1; i<=charCount; i++)
{
k=mark[charSet[i]];
while(k!=parent[k])
{
k=parent[k];
if(k<0)
k = -k, push(0);
else
push(1);
}
while((k=pop())> -1)
writeOnFile(k);
}
if(dc<7)
{
for(i=dc+1; i<8; i++)
s[i] = '0';
toDec();
fputc(7-dc+'0', fp);
}
else
{
fputc(0+'0', fp);
}
closeFile2Write();
}
void main()
{
//Informs error on code (0 - correct, 1 - error)
int fileError = 0;
//User choice
char choice;
//Creates the registers and instructions
createRegisters();
createInstructions();
while(1){
//Wait for a correct assembly file
while(assemblyChoice() == 0)
system("cls");
printf("\n*Press Y to assemble the file or other button to entry with other file*\n");
choice = getch();
if(choice == 'y'){
//Checks the assembly program
fileError = checksAssembly();
//If the program not has error
if(fileError == 0){
fileError = verifyAndTranslate();
if(fileError == 1){
//Calculates the program memory usage in Bytes
float memoryUsageB = ((numberOfData + numberOfInstructions)*4);
printf("\nPROGRAM ASSEMBLED\n");
printf("Number of instructions: %d\nNumber of data: %d\nMemory usage: %.4fKB\n", numberOfInstructions, numberOfData, memoryUsageB/1024);
writeOnFile();
}
}
printf("\n*Press any button to entry with other file*\n");
getch();
//Clean all past data
cleanCommands();
labels = NULL;
numberOfData = 0;
numberOfInstructions = 0;
fileError = 0;
}
system("cls");
}
}
void TrilaterationNode::wolfEcefCallback(const iri_common_drivers_msgs::NavSatFix_ecef::ConstPtr &msg)
{
//std::cout << "%%%% RAIM ecef = (" << msg->x << ", " << msg->y << ", " << msg->z << ")\n";
lastWolfECEF.setX(msg->x);
lastWolfECEF.setY(msg->y);
lastWolfECEF.setZ(msg->z);
if(saveOnDisk)
{
if(counterRaim==SAMPLING_RATE)
{
writeOnFile(PATH_WOLF_POS, ecefToLla(lastWolfECEF));
counterWolf = 0;
}
counterWolf++;
}
}
void TrilaterationNode::fixLlaCallback(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
std::cout << "%%%% FIX LLA = (" << msg->latitude << ", " << msg->longitude << ", " << msg->altitude << ")\n";
lastFixLLA.setX(msg->latitude * 180/M_PI);
lastFixLLA.setY(msg->longitude * 180/M_PI);
lastFixLLA.setZ(msg->altitude);
if(saveOnDisk)
{
if(counterLLA==SAMPLING_RATE)
{
writeOnFile(PATH_LLA, lastFixLLA);
counterLLA = 0;
}
counterLLA++;
}
}
void TrilaterationNode::pseudorangeCallback(const iri_common_drivers_msgs::SatellitePseudorangeArray::ConstPtr &msg)
{
std::cout << "Received " << msg->measurements.size() << " sat obs at " << msg->time_ros << "\n";
if(msg->measurements.size() < 4)
{
std::cout << "Not enough to trilaterate.\n";
return;
}
std::vector<SatelliteMeasurement> measurements;
for (int i = 0; i < msg->measurements.size(); ++i)
{
const iri_common_drivers_msgs::SatellitePseudorange sat = msg->measurements[i];
SatelliteMeasurement m = {
Point<double>(sat.x, sat.y, sat.z),
sat.pseudorange
};
measurements.push_back(m);
}
tr.setVerboseLevel(0);
Receiver estRec = tr.computePosition(measurements);
Point<double> estRecLLA = ecefToLla(estRec.pos);
std::cout << " ---> Estimation:\t" << estRec.toString() << std::endl;
std::cout << " real:\t " << lastFixECEF.toString() << std::endl;
std::cout << " diff coords:\t " << (estRec.pos + -lastFixECEF).toString() << std::endl;
std::cout << " error:\t " << estRec.pos.distanceTo(lastFixECEF) << std::endl;
std::cout << " raim error:\t " << estRec.pos.distanceTo(lastRaimECEF) << std::endl;
std::cout << " (LLA) real:\t " << (ecefToLla(lastFixECEF)).toString() << std::endl;
std::cout << " (LLA) est:\t " << estRecLLA.toString() << std::endl;
if(saveOnDisk)
{
if(counterEst==SAMPLING_RATE)
{
writeOnFile(PATH_EST_POS, estRecLLA);
counterEst = 0;
}
counterEst++;
}
//*************** debug purpose ********************
// std::cout << "\nRANGES:" << std::endl;
// double sum_r = 0, sum_e = 0;
// for (int i = 0; i < msg->measurements.size(); ++i)
// {
// const asterx1_node::SatPr sat = msg->measurements[i];
//
// Point<double> sat_pos(sat.x, sat.y, sat.z);
//
// double sq_r = pow(sat_pos.distanceTo(lastFixECEF) - sat.pseudorange, 2);
// double sq_e = pow(sat_pos.distanceTo(estRec.pos) - sat.pseudorange, 2);
//
// std::cout << "\t*(*)\t*sat" << sat.sat_id << ": " << sat.pseudorange << "\tpr sent" << std::endl;
// std::cout << "\t (R)\t-sat" << sat.sat_id << ": " << sat_pos.distanceTo(lastFixECEF) << "\trange with REAL pos" << std::endl;
// std::cout << "\t (E)\t sat" << sat.sat_id << ": " << sat_pos.distanceTo(estRec.pos) << "\trange with est pos" << std::endl;
//
// sum_r += sq_r;
// sum_e += sq_e;
// }
// std::cout << std::endl;
// std::cout << "\t*(R)\t*mean error " << sqrt(sum_r/msg->measurements.size()) << std::endl;
// std::cout << "\t (E)\t mean error " << sqrt(sum_e/msg->measurements.size()) << std::endl;
//************ END DEBUG PRINTINGS ***********
// Sets the guess for the next simulation in the actual position
//TODO vedi se serve effetivamente
tr.setInitialReceiverGuess(estRec);
// publish result
iri_common_drivers_msgs::NavSatFix_ecef estFixMsg;
//TODO fill up header etc
estFixMsg.x = estRec.pos.getX();
estFixMsg.y = estRec.pos.getY();
estFixMsg.z = estRec.pos.getZ();
estFixPub.publish(estFixMsg);
}
bool TrilaterationNode::writeOnFile(std::string path, Point<double> p)
{
return writeOnFile(path, p.getX(), p.getY(), p.getZ());
}