Select Git revision
dataplotter.py
example.cpp 6.11 KiB
#include "../src/HFS_API.hpp"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <chrono>
int main()
{
using std::chrono::high_resolution_clock;
using std::chrono::duration;
using std::chrono::milliseconds;
char path[] = "./src/HFS_config.xml";
char* real_path = realpath(path, NULL);
hfs_parameters update = hfs_parameters();
outputHfs outPacket = outputHfs();
unsigned int i, ctr_tra, ctr_acq;
double time;
double sum_x_tra, sum_y_tra, sum_x_acq, sum_y_acq; /*Values for mean*/
/*Quaternion to put WASP-11 in the center of the DRF*/
/*The resuling measurement of the */
//double quat[4] = {0.8689485897529942 , 0.36411458356110155 , -0.3351849018145181, 0.0};
double vel[3] = {0.0, 0.0, 0.0};
double quat[4] = {0.8549310535429379 , -0.3147579459933313 , -0.4123352145063788 , 0.0};
/*Initialise FGS simulator by calling constructor*/
FGS fgs(real_path);
free(real_path);
sum_x_acq = 0.;
sum_y_acq = 0.;
sum_x_tra = 0.;
sum_y_tra = 0.;
ctr_acq = 0;
ctr_tra = 0;
outPacket.time = 0.;
time = 0.;
/*Parameters of the hfs_parameters input packet*/
/*Reset FGS to original state of config*/
update.reset = 0;
/*set FGS channel*/
update.channel = 1;
/*
set FGS mode: In the 'set_params function' modes are set using integers.
This is done as Simulink doesn't support the char datatype
The equiviliant modes and numbers are:
1: Acquisition
2: Tracking
Other: Standby (will result in mode 0 in the output)
*/
update.mode = 1;
/*create xml file with current state*/
update.save = 0;
/*current simulation time of the update*/
//update.time = 0;
/*mark all cantroid packets as invalid*/
update.set_invalid = 0;
update.set_error = 0;
update.time = time;
/*set shifts*/
update.add_shift_x = 0;
update.add_shift_y = 0;
update.mult_shift_x = 1;
update.mult_shift_y = 1;
update.target_pos_x = 0;
update.target_pos_y = 0;
/*Set signal for target identification in ADU/s*/
update.validation_signal = (unsigned int) 55000*0.655;
/*Unit quaternion of the current SC attitude in reference to J2000*/
update.position_quat[0] = quat[0];
update.position_quat[1] = quat[1];
update.position_quat[2] = quat[2];
update.position_quat[3] = quat[3];
update.scVelocity[0] = vel[0];
update.scVelocity[1] = vel[1];
update.scVelocity[2] = vel[2];
/*Angular rate in the SC reference frame in arcsec/s*/
update.ang_rate[0] = 0;
update.ang_rate[1] = 0;
update.ang_rate[2] = 0;
auto t1 = high_resolution_clock::now();
for(i = 0; i < 10000; i++)
{
/*time step equivilant to 64Hz sampling*/
/*call update method of FGS class, centroid packet output is
updated once the measurement packet was sent*/
fgs.set_params(update, &outPacket);
/*Print measurement if it changed*/
update.time = update.time + 0.0125;
if(time != outPacket.time)
{
time = outPacket.time;
std::cout << "Packet time: " << outPacket.time << "\tx: " << outPacket.x << "\ty: " << outPacket.y << "\tMode: " << outPacket.mode << "\tvalidity: " << outPacket.validity_flag << "\tIndex: " << outPacket.validity_index << "\n";
sum_x_tra += outPacket.x;
sum_y_tra += outPacket.y;
ctr_tra++;
}
}
/*change mode*/
std::cout << "###################################\n";
std::cout << "Mode Change Tracking -> Acquisition\n";
std::cout << "###################################\n";
update.mode = 1;
/*Unit quaternion of the current SC attitude in reference to J2000*/
update.position_quat[0] = quat[0];
for(i = 0; i < 1000; i++)
{
fgs.set_params(update, &outPacket);
update.time = update.time + 0.0125;
if(time != outPacket.time)
{
time = outPacket.time;
std::cout << "Packet time: " << outPacket.time << "\tx: " << outPacket.x << "\ty: " << outPacket.y << "\tMode: " << outPacket.mode << "\tvalidity: " << outPacket.validity_flag << "\tIndex: " << outPacket.validity_index << "\n";
sum_x_acq += outPacket.x;
sum_y_acq += outPacket.y;
ctr_acq++;
}
}
/*Another mode change and centroids are set to invalid*/
std::cout << "###################################\n";
std::cout << "Mode Change Acquisition -> Tracking\n";
std::cout << "Centroids set to invalid\n";
std::cout << "###################################\n";
update.mode = 2;
update.set_invalid = 1;
for(i = 0; i < 1000; i++)
{
fgs.set_params(update, &outPacket);
update.time = update.time + 0.0125;
if(time != outPacket.time)
{
time = outPacket.time;
std::cout << "Packet time: " << outPacket.time << "\tx: " << outPacket.x << "\ty: " << outPacket.y << "\tMode: " << outPacket.mode << "\tvalidity: " << outPacket.validity_flag << "\tIndex: " << outPacket.validity_index << "\n";
}
}
std::cout << "###################################\n";
std::cout << "Mode Change Tracking -> Standby\n";
std::cout << "Centroids set to valid\n";
std::cout << "###################################\n";
update.mode = 0;
update.set_invalid = 0;
for(i = 0; i < 1000; i++)
{
fgs.set_params(update, &outPacket);
update.time = update.time + 0.0125;
if(time != outPacket.time)
{
time = outPacket.time;
std::cout << "Packet time: " << outPacket.time << "\tx: " << outPacket.x << "\ty: " << outPacket.y << "\tMode: " << outPacket.mode << "\tvalidity: " << outPacket.validity_flag << "\tIndex: " << outPacket.validity_index << "\n";
}
}
auto t2 = high_resolution_clock::now();
duration<double, std::milli> ms_double = t2 - t1;
std::cout << "\nSimulating " << outPacket.time << "s took: " << ms_double.count() << "ms\n";
std::cout << "Sim is " << outPacket.time / (ms_double.count()/1000) << " times faster than real time!\n";
std::cout << "Mean Measurement for Tracking: \n x: " << sum_x_tra/ctr_tra << "\n y: " << sum_y_tra/ctr_tra << "\n\n";
std::cout << "Tracking Reference Values: \n x: 463.047mas\n y: 279.941mas\n\n";
std::cout << "Mean Measurement for Acquisition: \n x: " << sum_x_acq/ctr_acq << "\n y: " << sum_y_acq/ctr_acq << "\n\n";
std::cout << "Acquisition Reference Values: \n x: 461.259mas\n y: 280.179mas\n\n";
}