void UploadFile::openIODevice() {
if (mIODevice.open(QIODevice::ReadOnly)) {
// get file size with file open
mLength = mIODevice.size();
} else {
Q_EMIT fileNotFound();
}
}
/// Reads potential data from a funcfl file and populates
/// corresponding members and InterpolationObjects in an EamPotential.
///
/// funcfl is a file format for tabulated potential functions used by
/// the original EAM code DYNAMO. A funcfl file contains an EAM
/// potential for a single element.
///
/// The contents of a funcfl file are:
///
/// | Line Num | Description
/// | :------: | :----------
/// | 1 | comments
/// | 2 | elem amass latConstant latType
/// | 3 | nrho drho nr dr rcutoff
/// | 4 | embedding function values F(rhobar) starting at rhobar=0
/// | ... | (nrho values. Multiple values per line allowed.)
/// | x' | electrostatic interation Z(r) starting at r=0
/// | ... | (nr values. Multiple values per line allowed.)
/// | y' | electron density values rho(r) starting at r=0
/// | ... | (nr values. Multiple values per line allowed.)
///
/// Where:
/// - elem : atomic number for this element
/// - amass : atomic mass for this element in AMU
/// - latConstant : lattice constant for this elemnent in Angstroms
/// - lattticeType : lattice type for this element (e.g. FCC)
/// - nrho : number of values for the embedding function, F(rhobar)
/// - drho : table spacing for rhobar
/// - nr : number of values for Z(r) and rho(r)
/// - dr : table spacing for r in Angstroms
/// - rcutoff : potential cut-off distance in Angstroms
///
/// funcfl format stores the "electrostatic interation" Z(r). This needs to
/// be converted to the pair potential phi(r).
/// using the formula
/// \f[phi = Z(r) * Z(r) / r\f]
/// NB: phi is not defined for r = 0
///
/// Z(r) is in atomic units (i.e., sqrt[Hartree * bohr]) so it is
/// necesary to convert to eV.
///
/// F(rhobar) is in eV.
///
void eamReadFuncfl(EamPotential* pot, const char* dir, const char* potName)
{
char tmp[4096];
sprintf(tmp, "%s/%s", dir, potName);
FILE* potFile = fopen(tmp, "r");
if (potFile == NULL)
fileNotFound("eamReadFuncfl", tmp);
// line 1
fgets(tmp, sizeof(tmp), potFile);
char name[3];
sscanf(tmp, "%s", name);
strcpy(pot->name, name);
// line 2
int nAtomic;
double mass, lat;
char latticeType[8];
fgets(tmp,sizeof(tmp),potFile);
sscanf(tmp, "%d %le %le %s", &nAtomic, &mass, &lat, latticeType);
pot->atomicNo = nAtomic;
pot->lat = lat;
pot->mass = mass*amuToInternalMass; // file has mass in AMU.
strcpy(pot->latticeType, latticeType);
// line 3
int nRho, nR;
double dRho, dR, cutoff;
fgets(tmp,sizeof(tmp),potFile);
sscanf(tmp, "%d %le %d %le %le", &nRho, &dRho, &nR, &dR, &cutoff);
pot->cutoff = cutoff;
real_t x0 = 0.0; // tables start at zero.
// allocate read buffer
int bufSize = MAX(nRho, nR);
real_t* buf = comdMalloc(bufSize * sizeof(real_t));
// read embedding energy
for (int ii=0; ii<nRho; ++ii)
fscanf(potFile, FMT1, buf+ii);
pot->f = initInterpolationObject(nRho, x0, dRho, buf);
// read Z(r) and convert to phi(r)
for (int ii=0; ii<nR; ++ii)
fscanf(potFile, FMT1, buf+ii);
for (int ii=1; ii<nR; ++ii)
{
real_t r = x0 + ii*dR;
buf[ii] *= buf[ii] / r;
buf[ii] *= hartreeToEv * bohrToAngs; // convert to eV
}
buf[0] = buf[1] + (buf[1] - buf[2]); // linear interpolation to get phi[0].
pot->phi = initInterpolationObject(nR, x0, dR, buf);
// read electron density rho
for (int ii=0; ii<nR; ++ii)
fscanf(potFile, FMT1, buf+ii);
pot->rho = initInterpolationObject(nR, x0, dR, buf);
comdFree(buf);
/* printPot(pot->f, "funcflDataF.txt"); */
/* printPot(pot->rho, "funcflDataRho.txt"); */
/* printPot(pot->phi, "funcflDataPhi.txt"); */
}
void SM_QDropbox::requestFinished(int nr, QNetworkReply *rply)
{
rply->deleteLater();
#ifdef SM_QTDROPBOX_DEBUG
int resp_bytes = rply->bytesAvailable();
#endif
QByteArray buff = rply->readAll();
QString response = QString(buff);
#ifdef SM_QTDROPBOX_DEBUG
qDebug() << "request " << nr << "finished." << endl;
qDebug() << "request was: " << rply->url().toString() << endl;
#endif
#ifdef SM_QTDROPBOX_DEBUG
qDebug() << "response: " << resp_bytes << "bytes" << endl;
qDebug() << "status code: " << rply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toString() << endl;
qDebug() << "== begin response ==" << endl << response << endl << "== end response ==" << endl;
qDebug() << "req#" << nr << " is of type " << requestMap[nr].type << endl;
#endif
// drop box error handling based on return codes
switch(rply->attribute(QNetworkRequest::HttpStatusCodeAttribute).toInt())
{
case SM_DROPBOX_ERROR_BAD_INPUT:
errorState = SM_QDropbox::BadInput;
errorText = "";
emit errorOccured(errorState);
checkReleaseEventLoop(nr);
return;
break;
case SM_DROPBOX_ERROR_EXPIRED_TOKEN:
errorState = SM_QDropbox::TokenExpired;
errorText = "";
emit tokenExpired();
checkReleaseEventLoop(nr);
return;
break;
case SM_DROPBOX_ERROR_BAD_OAUTH_REQUEST:
errorState = SM_QDropbox::BadOAuthRequest;
errorText = "";
emit errorOccured(errorState);
checkReleaseEventLoop(nr);
return;
break;
case SM_DROPBOX_ERROR_FILE_NOT_FOUND:
emit fileNotFound();
checkReleaseEventLoop(nr);
return;
break;
case SM_DROPBOX_ERROR_WRONG_METHOD:
errorState = SM_QDropbox::WrongHttpMethod;
errorText = "";
emit errorOccured(errorState);
checkReleaseEventLoop(nr);
return;
break;
case SM_DROPBOX_ERROR_REQUEST_CAP:
errorState = SM_QDropbox::MaxRequestsExceeded;
errorText = "";
emit errorOccured(errorState);
checkReleaseEventLoop(nr);
return;
break;
case SM_DROPBOX_ERROR_USER_OVER_QUOTA:
errorState = SM_QDropbox::UserOverQuota;
errorText = "";
emit errorOccured(errorState);
checkReleaseEventLoop(nr);
return;
break;
default:
break;
}
if(rply->error() != QNetworkReply::NoError)
{
errorState = SM_QDropbox::CommunicationError;
errorText = QString("%1 - %2").arg(rply->error()).arg(rply->errorString());
#ifdef SM_QTDROPBOX_DEBUG
qDebug() << "error " << errorState << "(" << errorText << ") in request" << endl;
#endif
emit errorOccured(errorState);
checkReleaseEventLoop(nr);
return;
}
// ignore connection requests
if(requestMap[nr].type == SM_DROPBOX_REQ_CONNECT)
{
#ifdef SM_QTDROPBOX_DEBUG
qDebug() << "- answer to connection request ignored" << endl;
#endif
removeRequestFromMap(nr);
return;
}
bool delayed_finish = false;
int delayed_nr;
if(rply->attribute(QNetworkRequest::HttpStatusCodeAttribute) == 302)
{
#ifdef SM_QTDROPBOX_DEBUG
qDebug() << "redirection received" << endl;
#endif
// redirection handling
QUrl newlocation(rply->header(QNetworkRequest::LocationHeader).toString(), QUrl::StrictMode);
#ifdef SM_QTDROPBOX_DEBUG
qDebug() << "new url: " << newlocation.toString() << endl;
#endif
int oldnr = nr;
nr = sendRequest(newlocation, requestMap[nr].method, 0, requestMap[nr].host);
requestMap[nr].type = SM_DROPBOX_REQ_REDIREC;
requestMap[nr].linked = oldnr;
return;
}
else
{
if(requestMap[nr].type == SM_DROPBOX_REQ_REDIREC)
{
// change values if this is the answert to a redirect
SM_DROPBOX_request redir = requestMap[nr];
SM_DROPBOX_request orig = requestMap[redir.linked];
requestMap[nr] = orig;
removeRequestFromMap(nr);
nr = redir.linked;
}
// standard handling depending on message type
switch(requestMap[nr].type)
{
case SM_DROPBOX_REQ_CONNECT:
// was only a connect request - so drop it
break;
case SM_DROPBOX_REQ_RQTOKEN:
// requested a tiken
responseTokenRequest(response);
break;
case SM_DROPBOX_REQ_RQBTOKN:
responseBlockedTokenRequest(response);
break;
case SM_DROPBOX_REQ_AULOGIN:
delayed_nr = responseDropboxLogin(response, nr);
delayed_finish = true;
break;
case SM_DROPBOX_REQ_ACCTOKN:
responseAccessToken(response);
break;
case SM_DROPBOX_REQ_METADAT:
parseMetadata(response);
break;
case SM_DROPBOX_REQ_BMETADA:
parseBlockingMetadata(response);
break;
case SM_DROPBOX_REQ_BACCTOK:
responseBlockingAccessToken(response);
break;
case SM_DROPBOX_REQ_ACCINFO:
parseAccountInfo(response);
break;
case SM_DROPBOX_REQ_BACCINF:
parseBlockingAccountInfo(response);
break;
case SM_DROPBOX_REQ_SHRDLNK:
parseSharedLink(response);
break;
case SM_DROPBOX_REQ_BSHRDLN:
parseBlockingSharedLink(response);
break;
case SM_DROPBOX_REQ_REVISIO:
parseRevisions(response);
break;
case SM_DROPBOX_REQ_BREVISI:
parseBlockingRevisions(response);
break;
case SM_DROPBOX_REQ_DELTA:
parseDelta(response);
break;
case SM_DROPBOX_REQ_BDELTA:
parseBlockingDelta(response);
break;
default:
errorState = SM_QDropbox::ResponseToUnknownRequest;
errorText = "Received a response to an unknown request";
emit errorOccured(errorState);
break;
}
}
if(delayed_finish)
delayMap[delayed_nr] = nr;
else
{
if(delayMap[nr])
{
int drq = delayMap[nr];
while(drq!=0)
{
emit operationFinished(delayMap[drq]);
delayMap.remove(drq);
drq = delayMap[drq];
}
}
removeRequestFromMap(nr);
emit operationFinished(nr);
}
return;
}
/// Reads potential data from a setfl file and populates
/// corresponding members and InterpolationObjects in an EamPotential.
///
/// setfl is a file format for tabulated potential functions used by
/// the original EAM code DYNAMO. A setfl file contains EAM
/// potentials for multiple elements.
///
/// The contents of a setfl file are:
///
/// | Line Num | Description
/// | :------: | :----------
/// | 1 - 3 | comments
/// | 4 | ntypes type1 type2 ... typen
/// | 5 | nrho drho nr dr rcutoff
/// | F, rho | Following line 5 there is a block for each atom type with F, and rho.
/// | b1 | ielem(i) amass(i) latConst(i) latType(i)
/// | b2 | embedding function values F(rhobar) starting at rhobar=0
/// | ... | (nrho values. Multiple values per line allowed.)
/// | bn | electron density, starting at r=0
/// | ... | (nr values. Multiple values per line allowed.)
/// | repeat | Return to b1 for each atom type.
/// | phi | phi_ij for (1,1), (2,1), (2,2), (3,1), (3,2), (3,3), (4,1), ...,
/// | p1 | pair potential between type i and type j, starting at r=0
/// | ... | (nr values. Multiple values per line allowed.)
/// | repeat | Return to p1 for each phi_ij
///
/// Where:
/// - ntypes : number of element types in the potential
/// - nrho : number of points the embedding energy F(rhobar)
/// - drho : table spacing for rhobar
/// - nr : number of points for rho(r) and phi(r)
/// - dr : table spacing for r in Angstroms
/// - rcutoff : cut-off distance in Angstroms
/// - ielem(i) : atomic number for element(i)
/// - amass(i) : atomic mass for element(i) in AMU
/// - latConst(i) : lattice constant for element(i) in Angstroms
/// - latType(i) : lattice type for element(i)
///
/// setfl format stores r*phi(r), so we need to converted to the pair
/// potential phi(r). In the file, phi(r)*r is in eV*Angstroms.
/// NB: phi is not defined for r = 0
///
/// F(rhobar) is in eV.
///
void eamReadSetfl(EamPotential* pot, const char* dir, const char* potName)
{
char tmp[4096];
sprintf(tmp, "%s/%s", dir, potName);
FILE* potFile = fopen(tmp, "r");
if (potFile == NULL)
fileNotFound("eamReadSetfl", tmp);
// read the first 3 lines (comments)
fgets(tmp, sizeof(tmp), potFile);
fgets(tmp, sizeof(tmp), potFile);
fgets(tmp, sizeof(tmp), potFile);
// line 4
fgets(tmp, sizeof(tmp), potFile);
int nElems;
sscanf(tmp, "%d", &nElems);
if( nElems != 1 )
notAlloyReady("eamReadSetfl");
//line 5
int nRho, nR;
double dRho, dR, cutoff;
// The same cutoff is used by all alloys, NB: cutoff = nR * dR is redundant
fgets(tmp, sizeof(tmp), potFile);
sscanf(tmp, "%d %le %d %le %le", &nRho, &dRho, &nR, &dR, &cutoff);
pot->cutoff = cutoff;
// **** THIS CODE IS RESTRICTED TO ONE ELEMENT
// Per-atom header
fgets(tmp, sizeof(tmp), potFile);
int nAtomic;
double mass, lat;
char latticeType[8];
sscanf(tmp, "%d %le %le %s", &nAtomic, &mass, &lat, latticeType);
pot->atomicNo = nAtomic;
pot->lat = lat;
pot->mass = mass * amuToInternalMass; // file has mass in AMU.
strcpy(pot->latticeType, latticeType);
// allocate read buffer
int bufSize = MAX(nRho, nR);
real_t* buf = comdMalloc(bufSize * sizeof(real_t));
real_t x0 = 0.0;
// Read embedding energy F(rhobar)
for (int ii=0; ii<nRho; ++ii)
fscanf(potFile, FMT1, buf+ii);
pot->f = initInterpolationObject(nRho, x0, dRho, buf);
// Read electron density rho(r)
for (int ii=0; ii<nR; ++ii)
fscanf(potFile, FMT1, buf+ii);
pot->rho = initInterpolationObject(nR, x0, dR, buf);
// Read phi(r)*r and convert to phi(r)
for (int ii=0; ii<nR; ++ii)
fscanf(potFile, FMT1, buf+ii);
for (int ii=1; ii<nR; ++ii)
{
real_t r = x0 + ii*dR;
buf[ii] /= r;
}
buf[0] = buf[1] + (buf[1] - buf[2]); // Linear interpolation to get phi[0].
pot->phi = initInterpolationObject(nR, x0, dR, buf);
comdFree(buf);
// write to text file for comparison, currently commented out
/* printPot(pot->f, "SetflDataF.txt"); */
/* printPot(pot->rho, "SetflDataRho.txt"); */
/* printPot(pot->phi, "SetflDataPhi.txt"); */
}
int main(void)
{
int sock, sock2;
struct sockaddr_in echoclient,from, ctrlclient, ctrlinclient;
unsigned int echolen, clientlen;
int received = 0;
FGNetFDM buf;
char* buffer = (char*)&buf;
FGNetCtrls bufctrl;
char* buffer_ctrl = (char*)&bufctrl;
struct known_state state;
struct known_state old_state;
struct known_state_ints state_ints;
struct known_state_ints old_state_ints;
srand ( time(NULL) );
memset(&state, 0, sizeof(state));
memset(&old_state, 0, sizeof(state));
memset(&state_ints, 0, sizeof(state_ints));
memset(&old_state_ints, 0, sizeof(state_ints));
/* Create the UDP socket */
if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
std::cerr << "Failed to create socket" << std::endl;
perror("socket");
return -1;
}
if ((sock2 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
std::cerr << "Failed to create socket" << std::endl;
perror("socket");
return -1;
}
/// the port where net fdm is received
memset(&echoclient, 0, sizeof(echoclient)); /* Clear struct */
echoclient.sin_family = AF_INET; /* Internet/IP */
echoclient.sin_addr.s_addr = inet_addr("127.0.0.1"); /* IP address */
echoclient.sin_port = htons(5500); /* server port */
/// the port where the modified net ctrl is sent to flightgear
memset(&ctrlclient, 0, sizeof(ctrlclient)); /* Clear struct */
ctrlclient.sin_family = AF_INET; /* Internet/IP */
ctrlclient.sin_addr.s_addr = inet_addr("127.0.0.1"); /* IP address */
ctrlclient.sin_port = htons(5600); /* server port */
/// this is the port where the net ctrl comes from flight gear
memset(&ctrlinclient, 0, sizeof(ctrlinclient)); /* Clear struct */
ctrlinclient.sin_family = AF_INET; /* Internet/IP */
ctrlinclient.sin_addr.s_addr = inet_addr("127.0.0.1"); /* IP address */
ctrlinclient.sin_port = htons(5700); /* server port */
int length = sizeof(buf);
std::cout << "packet length should be " << length << " bytes" << std::endl;
echolen = length;
char errorbuf[100];
/// need this?
int ret = bind(sock, (struct sockaddr*)&echoclient, sizeof(echoclient) );
int ret2 = bind(sock2, (struct sockaddr*)&ctrlinclient, sizeof(ctrlinclient) );
std::cout << "bind " << ret << " " << ret2 << std::endl;
///////////////////////////////////////////////////////////////////
std::string file = "telemetry.csv";
std::ofstream telem(file.c_str(), std::ios_base::out );
if (!telem) {
std::cout << "File \"" << file << "\" not found.\n";
throw fileNotFound();
return 2;
}
telem.precision(10);
telem <<
"longitude, latitude, altitude" <<
"p, q, r," <<
"elevator, rudder, aileron" <<
std::endl;
//std::string binfile = "sim_telemetry.bin";
//std::ofstream myFile(binfile, std::ios::out | std::ios::binary);
double time = 0.0;
float dist = 40e3; //state.altitude;
float div = 3e7;
double target_longitude = -2.137;
double target_latitude = .658;
double target_altitude = 50.0; //meters
std::cout << "struct size " << sizeof(state) << std::endl;
FILE* telem_file;
telem_file = fopen("telem.bin","wb");
int i = 0;
while(1) {
i++;
// I think this clobbers echoclient
/// receive net_fdm over udp
received = recvfrom(sock, buffer, sizeof(buf), 0,
(struct sockaddr *) &from,
&echolen);
/// -extract accelerations A_X_pilot etc, add noise
/// - are phidot, thetadot, psidot in body frame? Probably
/// (I thought someone told me there was no difference, but that doesn't
/// make sense- what if the vehicle was 90 degrees up from normal, how is
/// roll still roll?)
/// - take position lat long alt, but filter so it is only updated at a few Hz
/// and that's all we'll know
FGNetFDM2Props( &buf);
if (i == 1) {
target_longitude = buf.longitude + (float)(rand()%(int)dist)/div - dist/div*0.5;
target_latitude = buf.latitude + (float)(rand()%(int)dist)/div - dist/div*0.5;
std::cout << "t longitude=" << target_longitude << ", t latitude=" << target_latitude << std::endl;
}
if (i%10 == 0) {
state.longitude= (float)((int)(180e4/M_PI*buf.longitude))/(180e4/M_PI);;
state.latitude = (float)((int)(180e4/M_PI*buf.latitude))/(180e4/M_PI);
state.altitude = buf.altitude*M2FT;
} else {
state.longitude= old_state.longitude;
state.latitude = old_state.latitude;
state.altitude = old_state.altitude;
}
state.p = FPFIX(buf.p);
state.q = FPFIX(buf.q);
state.r = FPFIX(buf.r);
state.A_X_pilot = FPFIX(buf.A_X_pilot);
state.A_Y_pilot = FPFIX(buf.A_Y_pilot);
state.A_Z_pilot = FPFIX(buf.A_Z_pilot);
state.target_long= target_longitude;
state.target_lat = target_latitude;
state.target_alt = target_altitude;
#if 0
/// fixed point version
if (j%10 == 0) {
/// convert to arc-minutes
state_ints.longitude = float_to_fixed(180.0*60.0/M_PI*buf.longitude);
state_ints.latitude = float_to_fixed(180.0*60.0/M_PI*buf.latitude);
/// convert to feet later?
state_ints.altitude = (int)(buf.altitude);
/// TEMP
/// alternate way of getting velocity- this one matches the sim
double x1 = (EARTH_RADIUS_METERS+state.altitude)*cos(state.latitude)*cos(state.longitude);
double y1 = (EARTH_RADIUS_METERS+state.altitude)*cos(state.latitude)*sin(state.longitude);
double z1 = (EARTH_RADIUS_METERS+state.altitude)*sin(state.latitude);
double x2 = (EARTH_RADIUS_METERS+old_state.altitude)*cos(old_state.latitude)*cos(old_state.longitude);
double y2 = (EARTH_RADIUS_METERS+old_state.altitude)*cos(old_state.latitude)*sin(old_state.longitude);
double z2 = (EARTH_RADIUS_METERS+old_state.altitude)*sin(old_state.latitude);
// delta xyz to target
double xt = (EARTH_RADIUS_METERS+target_altitude)*cos(target_latitude)*cos(target_longitude);
double yt = (EARTH_RADIUS_METERS+target_altitude)*cos(target_latitude)*sin(target_longitude);
double zt = (EARTH_RADIUS_METERS+target_altitude)*sin(target_latitude);
double dx = (x1-x2);
double dy = (y1-y2);
double dz = (z1-z2);
// length of vector pointing from old location to current
float l1 = sqrtf(dx*dx + dy*dy + dz*dz);
/// length of vector point straight at center of earth
double l2 = sqrtf(x2*x2 + y2*y2 + z2*z2);
std::cout << "velocity correct " << l1
<< ", dx " << (dx);
// << ", radius " << EARTH_RADIUS_METERS
// << ", coslat " << cos(state.latitude)*cos(state.longitude);
//<< ", dy " << (y1)
//<< ", dz " << (z1);
/*
double dotprod = (
(dx/l1 * (-x2)/l2) +
(dy/l1 * (-y2)/l2) +
(dz/l1 * (-z2)/l2) );
state.pitch = acos(dotprod)/M_PI -0.5;
*/
}
j++;
state_ints.target_long= float_to_fixed(target_longitude);
state_ints.target_lat = float_to_fixed(target_latitude);
state_ints.target_alt = float_to_fixed(target_altitude);
state_ints.p = float_to_fixed(buf.p);
state_ints.q = float_to_fixed(buf.q);
state_ints.r = float_to_fixed(buf.r);
state_ints.A_X_pilot = float_to_fixed(buf.A_X_pilot);
state_ints.A_Y_pilot = float_to_fixed(buf.A_Y_pilot);
state_ints.A_Z_pilot = float_to_fixed(buf.A_Z_pilot);
#endif
if (received < 0) {
perror("recvfrom");
// } else if (received != echolen) {
// std::cerr << "wrong sized packet " << received << std::endl;
} else {
/// get the ctrls that are properly filled out with environmental
/// data, so we don't clobber that when we modify the flap positions
received = recvfrom(sock2, buffer_ctrl, sizeof(bufctrl), 0,
(struct sockaddr *) &ctrlinclient,
&echolen);
FGProps2NetCtrls(&bufctrl);
float dt =1.0/10.0;
autopilot(state, old_state,dt);
bufctrl.elevator = FPFIX(state.elevator);
bufctrl.aileron = FPFIX(state.aileron);
bufctrl.rudder = FPFIX(state.rudder);
//autopilot_ints(state_ints, old_state_ints, buf, bufctrl);
//state_fixed_to_float(state_ints,state);
/// output to file and stdout
{
/// save telemetry to file
telem <<
buf.longitude << "," << buf.latitude << "," << buf.altitude*M2FT << "," <<
state.p << "," << state.q << "," << state.r << "," <<
bufctrl.elevator << "," << bufctrl.rudder << "," << bufctrl.aileron << "," <<
state.dq << "," << state.iq << "," <<
state.error_heading << "," << state.derror_heading << "," << state.ierror_heading << "," <<
state.error_pitch << "," << state.dpitch << "," << state.ipitch << "," <<
state.tpitch << "," << state.speed << "," <<
state.tdx << "," << state.tdy << "," <<
bufctrl.wind_speed_kt << "," << bufctrl.wind_dir_deg << "," << bufctrl.press_inhg << "," <<
state.dr << "," << state.ir << "," << state.pitch <<
state.A_X_pilot << "," << state.A_Y_pilot << "," << state.A_Z_pilot << "," <<
state.longitude << "," << state.latitude << "," << state.altitude << "," <<
std::endl;
/// output some of the state to stdout every few seconds
static int i = 0;
i++;
std::cout.precision(2);
//if ((state.longitude != old_state.longitude ) || (state.latitude != old_state.latitude)) {
if (i%30==0) {
std::cout <<
", hor dist=" << sqrtf(state.tdist*state.tdist - (state.altitude-target_altitude)*(state.altitude-target_altitude)) <<
//", agl=" << buf.agl << ", alt=" << state.altitude << ", vel=" << state.speed <<
// ", fdm v= " << sqrtf(buf.v_north*buf.v_north + buf.v_east*buf.v_east + buf.v_down*buf.v_down)*0.3048 <<
// " tdx=" << state.tdx << ", tdy=" << state.tdy <<
// " heading= " << heading << " target heading= " << theading <<
", err_head= " << state.error_heading << ", rud=" << bufctrl.rudder <<
// ", lat= " << state.latitude << ", long= " << state.longitude << ", alt= " << state.altitude <<
", pitch= " << state.pitch << ", tpitch= " << state.tpitch << ", elev=" << bufctrl.elevator <<
// ", p=" << state.p <<
// ", q=" << state.q << ", elev=" << bufctrl.elevator <<
", r=" << state.r << ", ail= " << bufctrl.aileron <<
", ax=" << state.A_X_pilot << ", ay=" << state.A_Y_pilot << ", az=" << state.A_Z_pilot <<
", orient=" << state.acc_orientation <<
std::endl;
}
}
fwrite((void*)&state, 1, sizeof(state), telem_file);
//std::cout << old_state.altitude << " " << state.altitude << std::endl;
memcpy(&old_state, &state, sizeof(state));
//memcpy(&old_state_ints, &state_ints, sizeof(state_ints));
////////////////////////////////////////////////////////////////////////
received = sendto(sock, buffer_ctrl, sizeof(bufctrl), 0,
(struct sockaddr *) &ctrlclient, sizeof(ctrlclient));
if (received <0) {
perror("sendto");
}
time += 0.1;
}
usleep(3000);
}
fclose(telem_file);
return 0;
}
