int main() {
char buffer[BUFFERSIZE];
char **path = init_path();
int path_length = 1;
while (1) {
char *filename = 0;
printf("%s", "whoosh > ");
if (fgets(buffer, BUFFERSIZE, stdin) != NULL) {
int command_length = 0;
char **commands = tokenize_string(trim_whitespace(buffer), &command_length);
if (check_if_built_in_command(commands, command_length) != 0) {
if (strcmp(commands[0], "exit") == 0)
exit(0);
else if (strcmp(commands[0], "pwd") == 0) {
char buff[PATH_MAX + 1];
printf("%s\n", getcwd(buff, PATH_MAX + 1));
}
else if (strcmp(commands[0], "cd") == 0) {
char *dir = command_length == 1 ? getenv("HOME") : commands[1];
if (chdir(dir) != 0)
error_output();
}
else if (strcmp(commands[0], "path") == 0) {
path = commands + 1;
path_length = command_length - 1;
}
}
else if (command_length != 0 && (check_command_syntax(commands, command_length, &filename) != 0 ||
run_shell_commands(commands, command_length, path, path_length, filename) != 0))
error_output();
}
else
error_output();
}
}
int Command::run(const std::vector<std::string>& params)
{
try
{
m_args.reset(new Argument_Parser(m_spec,
{"verbose", "help"},
{"output", "error-output", "rng-type", "drbg-seed"}));
m_args->parse_args(params);
if(m_args->has_arg("output"))
{
const std::string output_file = get_arg("output");
if(output_file != "")
{
m_output_stream.reset(new std::ofstream(output_file, std::ios::binary));
if(!m_output_stream->good())
throw CLI_IO_Error("opening", output_file);
}
}
if(m_args->has_arg("error-output"))
{
const std::string output_file = get_arg("error-output");
if(output_file != "")
{
m_error_output_stream.reset(new std::ofstream(output_file, std::ios::binary));
if(!m_error_output_stream->good())
throw CLI_IO_Error("opening", output_file);
}
}
if(flag_set("help"))
{
output() << help_text() << "\n";
return 2;
}
this->go();
return m_return_code;
}
catch(CLI_Usage_Error& e)
{
error_output() << "Usage error: " << e.what() << "\n";
error_output() << help_text() << "\n";
return 1;
}
catch(std::exception& e)
{
error_output() << "Error: " << e.what() << "\n";
return 2;
}
catch(...)
{
error_output() << "Error: unknown exception\n";
return 2;
}
}
std::string Command::get_passphrase(const std::string& prompt)
{
if(echo_suppression_supported() == false)
error_output() << "Warning: terminal echo suppression not enabled for this platform\n";
error_output() << prompt << ": " << std::flush;
std::string pass;
auto echo_suppress = Botan::OS::suppress_echo_on_terminal();
std::getline(std::cin, pass);
return pass;
}
void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
{
// realign all the filter if needed
// a complete init cycle is required
if (ahrs_float_inv.reset) {
ahrs_float_inv.reset = false;
ahrs_float_inv.is_aligned = false;
init_invariant_state();
}
// fill command vector
struct FloatRates gyro_meas_body;
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
ahrs_float_inv.cmd.rates = gyro_meas_body;
// update correction gains
error_output(&ahrs_float_inv);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float *)&new_state,
(float *)&ahrs_float_inv.state, INV_STATE_DIM,
(float *)&ahrs_float_inv.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ahrs_float_inv.state = new_state;
// normalize quaternion
float_quat_normalize(&ahrs_float_inv.state.quat);
//------------------------------------------------------------//
#if SEND_INVARIANT_FILTER
struct FloatEulers eulers;
float foo = 0.f;
float_eulers_of_quat(&eulers, &ahrs_float_inv.state.quat);
RunOnceEvery(3,
pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
AC_ID,
&ahrs_float_inv.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&foo,
&foo,
&foo,
&foo,
&foo,
&foo,
&ahrs_float_inv.state.bias.p,
&ahrs_float_inv.state.bias.q,
&ahrs_float_inv.state.bias.r,
&ahrs_float_inv.state.as,
&ahrs_float_inv.state.cs,
&foo,
&foo);
);
static void freeNtfFilter(SaNtfNotificationFilterHandleT *fh_ptr)
{
SaAisErrorT errorCode = SA_AIS_OK;
if (*fh_ptr) {
errorCode = saNtfNotificationFilterFree(*fh_ptr);
if (SA_AIS_OK != errorCode) {
fprintf(stderr, "saNtfNotificationFilterFree failed - %s\n", error_output(errorCode));
exit(EXIT_FAILURE);
}
}
}
int main(void){
my_test();
int errcheck = 0;
int *ERR = &errcheck;
section_t *fourteen = section_new(ERR);
if(error_output(ERR))
return 0;
section_auditory_add(fourteen, 67, 25, ERR);
if(error_output(ERR))
return 0;
int plc = section_total_places(fourteen, ERR);
if(error_output(ERR))
return 0;
printf("\nPlaces: %d", plc);
auditory_occupy(fourteen, 67, "Hadyniak", ERR);
if(error_output(ERR))
return 0;
char *name = auditory_isoccupied(fourteen, 67, ERR);
if(error_output(ERR))
return 0;
printf("Auditory 67-14 is occupied by %s", name);
return 0;
}
void ahrs_propagate(void) {
struct NedCoor_f accel;
struct FloatRates body_rates;
struct FloatEulers eulers;
// realign all the filter if needed
// a complete init cycle is required
if (ins_impl.reset) {
ins_impl.reset = FALSE;
ins.status = INS_UNINIT;
ahrs.status = AHRS_UNINIT;
init_invariant_state();
}
// fill command vector
struct Int32Rates gyro_meas_body;
INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro);
RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
struct Int32Vect3 accel_meas_body;
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);
// update correction gains
error_output(&ins_impl);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float*)&new_state,
(float*)&ins_impl.state, INV_STATE_DIM,
(float*)&ins_impl.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ins_impl.state = new_state;
// normalize quaternion
FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);
// set global state
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
// Some stupid lines of code for neutrals
eulers.phi -= ins_roll_neutral;
eulers.theta -= ins_pitch_neutral;
stateSetNedToBodyEulers_f(&eulers);
#else
stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
stateSetBodyRates_f(&body_rates);
stateSetPositionNed_f(&ins_impl.state.pos);
stateSetSpeedNed_f(&ins_impl.state.speed);
// untilt accel and remove gravity
FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
FLOAT_VECT3_ADD(accel, A);
stateSetAccelNed_f(&accel);
//------------------------------------------------------------//
RunOnceEvery(3,{
DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
&ins_impl.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&ins_impl.state.speed.x,
&ins_impl.state.speed.y,
&ins_impl.state.speed.z,
&ins_impl.state.pos.x,
&ins_impl.state.pos.y,
&ins_impl.state.pos.z,
&ins_impl.state.bias.p,
&ins_impl.state.bias.q,
&ins_impl.state.bias.r,
&ins_impl.state.as,
&ins_impl.state.hb,
&ins_impl.meas.baro_alt,
&ins_impl.meas.pos_gps.z)
});
#if LOG_INVARIANT_FILTER
if (pprzLogFile.fs != NULL) {
if (!log_started) {
// log file header
sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
log_started = TRUE;
}
else {
sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
ins_impl.cmd.rates.p,
ins_impl.cmd.rates.q,
ins_impl.cmd.rates.r,
ins_impl.cmd.accel.x,
ins_impl.cmd.accel.y,
ins_impl.cmd.accel.z,
ins_impl.meas.pos_gps.x,
ins_impl.meas.pos_gps.y,
ins_impl.meas.pos_gps.z,
ins_impl.meas.speed_gps.x,
ins_impl.meas.speed_gps.y,
ins_impl.meas.speed_gps.z,
ins_impl.meas.mag.x,
ins_impl.meas.mag.y,
ins_impl.meas.mag.z,
ins_impl.meas.baro_alt,
ins_impl.state.quat.qi,
ins_impl.state.quat.qx,
ins_impl.state.quat.qy,
ins_impl.state.quat.qz,
ins_impl.state.bias.p,
ins_impl.state.bias.q,
ins_impl.state.bias.r,
ins_impl.state.speed.x,
ins_impl.state.speed.y,
ins_impl.state.speed.z,
ins_impl.state.pos.x,
ins_impl.state.pos.y,
ins_impl.state.pos.z,
ins_impl.state.hb,
ins_impl.state.as);
}
}
#endif
}
int main(int argc, char* argv[])
#endif
{
setlocale(LC_NUMERIC, "C");
std::stringstream dummy;
PATHMANAGER::Init(dummy, dummy);
/// open Log file
//----------------------------------------------------------------
std::string logfilename = PATHMANAGER::GetLogDir() + "/log.txt";
std::ofstream logfile(logfilename.c_str());
if (!logfile)
{
std::cerr << "Couldn't open log file: " << logfilename << std::endl;
return EXIT_FAILURE;
}
// set up logging arrangement
logging::splitterstreambuf infosplitter(std::cout, logfile);
std::ostream infosplitterstream(&infosplitter);
logging::splitterstreambuf errorsplitter(std::cerr, logfile);
std::ostream errorsplitterstream(&errorsplitter);
logging::logstreambuf infolog("INFO: ", infosplitterstream); //logstreambuf infolog("INFO: ", logfile);
logging::logstreambuf errorlog("ERROR: ", errorsplitterstream);
// primary logging ostreams
std::ostream info_output(&infolog);
std::ostream error_output(&errorlog);/**/
/// Load Settings
//----------------------------------------------------------------
SETTINGS* settings = new SETTINGS();
std::string setFile = PATHMANAGER::GetSettingsFile();
if (!PATHMANAGER::FileExists(setFile))
{
info_output << "Settings not found - loading defaults." << std::endl;
LoadDefaultSet(settings,setFile);
}
settings->Load(setFile); // LOAD
if (settings->version != SET_VER) // loaded older, use default
{
info_output << "Settings found, but older version - loading defaults." << std::endl;
LoadDefaultSet(settings,setFile);
settings->Load(setFile); // LOAD
}
// HACK: we initialize paths a second time now that we have the output streams
PATHMANAGER::Init(info_output, error_output);
/// Game start
//----------------------------------------------------------------
GAME* pGame = new GAME(info_output, error_output, settings);
std::list <std::string> args;//(argv, argv + argc);
pGame->Start(args); //game.End();
App* pApp = new App();
pApp->pSet = settings;
pApp->pGame = pGame;
pGame->pOgreGame = pApp;
try
{
if (settings->multi_thr > 0)
boost::thread t(VprThread, pApp);
#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
pApp->Run( settings->ogre_dialog || lpCmdLine[0]!=0 ); //Release change-
#else
pApp->Run( settings->ogre_dialog);
#endif
}
catch (Ogre::Exception& e)
{
#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
MessageBoxA( NULL, e.getFullDescription().c_str(), "An exception has occured!", MB_OK | MB_ICONERROR | MB_TASKMODAL);
#else
std::cerr << "An exception has occured: " << e.getFullDescription().c_str() << std::endl;
#endif
}
info_output << "Exiting" << std::endl;
delete pApp;
delete pGame;
delete settings;
return 0;
}
int main(int argc, char *argv[])
{
int c;
SaAisErrorT error;
int timeout = -1; /* block indefintively in poll */
saNotificationFilterAllocationParamsT notificationFilterAllocationParams = {0};
SaNtfSubscriptionIdT subscriptionId = 1;
struct option long_options[] = {
{"alarm", no_argument, 0, 'a'},
{"attributeChange", no_argument, 0, 'c'},
{"objectCreateDelete", no_argument, 0, 'o'},
{"stateChange", no_argument, 0, 's'},
{"securityAlarm", no_argument, 0, 'y'},
{"help", no_argument, 0, 'h'},
{"timeout", required_argument, 0, 't'},
{"verbose", no_argument, 0, 'v'},
{0, 0, 0, 0}
};
progname = argv[0];
/* Check options */
while (1) {
c = getopt_long(argc, argv, "acosyht:v", long_options, NULL);
if (c == -1)
break;
switch (c) {
case 'a':
used_filters.all = 0;
used_filters.alarm = 1;
break;
case 'o':
used_filters.all = 0;
used_filters.obj_cr_del = 1;
break;
case 'c':
used_filters.all = 0;
used_filters.att_ch = 1;
break;
case 's':
used_filters.all = 0;
used_filters.st_ch = 1;
break;
case 'y':
used_filters.all = 0;
used_filters.sec_al = 1;
break;
case 't':
timeout = atoi(optarg) * 1000;
break;
case 'v':
verbose = 1;
break;
case 'h':
case '?':
default:
usage();
break;
}
}
error = saNtfInitialize(&ntfHandle, &ntfCallbacks, &version);
if (SA_AIS_OK != error) {
fprintf(stderr, "saNtfInitialize failed - %s\n", error_output(error));
exit(EXIT_FAILURE);
}
error = saNtfSelectionObjectGet(ntfHandle, &selObj);
if (SA_AIS_OK != error) {
fprintf(stderr, "saNtfSelectionObjectGet failed - %s\n", error_output(error));
exit(EXIT_FAILURE);
}
error = subscribeForNotifications(¬ificationFilterAllocationParams, subscriptionId);
if (SA_AIS_OK != error) {
fprintf(stderr, "subscribeForNotifications failed - %s\n", error_output(error));
exit(EXIT_FAILURE);
}
error = waitForNotifications(ntfHandle, selObj, timeout);
if (SA_AIS_OK != error) {
fprintf(stderr, "subscribeForNotifications failed - %s\n", error_output(error));
exit(EXIT_FAILURE);
}
error = saNtfNotificationUnsubscribe(subscriptionId);
if (SA_AIS_OK != error) {
fprintf(stderr, "waitForNotifications failed - %s\n", error_output(error));
exit(EXIT_FAILURE);
}
error = saNtfFinalize(ntfHandle);
if (SA_AIS_OK != error) {
fprintf(stderr, "saNtfFinalize failed - %s\n", error_output(error));
exit(EXIT_FAILURE);
}
exit(EXIT_SUCCESS);
}
/* Subscribe */
static SaAisErrorT subscribeForNotifications(const saNotificationFilterAllocationParamsT
*notificationFilterAllocationParams, SaNtfSubscriptionIdT subscriptionId)
{
SaAisErrorT errorCode = SA_AIS_OK;
SaNtfAlarmNotificationFilterT myAlarmFilter;
SaNtfAttributeChangeNotificationFilterT attChFilter;
SaNtfStateChangeNotificationFilterT stChFilter;
SaNtfObjectCreateDeleteNotificationFilterT objCrDelFilter;
SaNtfSecurityAlarmNotificationFilterT secAlarmFilter;
SaNtfNotificationTypeFilterHandlesT notificationFilterHandles;
memset(¬ificationFilterHandles, 0, sizeof notificationFilterHandles);
if (used_filters.all || used_filters.alarm) {
errorCode = saNtfAlarmNotificationFilterAllocate(ntfHandle,
&myAlarmFilter,
notificationFilterAllocationParams->numEventTypes,
notificationFilterAllocationParams->numNotificationObjects,
notificationFilterAllocationParams->numNotifyingObjects,
notificationFilterAllocationParams->numNotificationClassIds,
notificationFilterAllocationParams->numProbableCauses,
notificationFilterAllocationParams->numPerceivedSeverities,
notificationFilterAllocationParams->numTrends);
if (errorCode != SA_AIS_OK) {
fprintf(stderr, "saNtfAlarmNotificationFilterAllocate failed - %s\n", error_output(errorCode));
return errorCode;
}
notificationFilterHandles.alarmFilterHandle = myAlarmFilter.notificationFilterHandle;
}
if (used_filters.all || used_filters.att_ch) {
errorCode = saNtfAttributeChangeNotificationFilterAllocate(ntfHandle,
&attChFilter,
notificationFilterAllocationParams->numEventTypes,
notificationFilterAllocationParams->numNotificationObjects,
notificationFilterAllocationParams->numNotifyingObjects,
notificationFilterAllocationParams->numNotificationClassIds,
0);
if (errorCode != SA_AIS_OK) {
fprintf(stderr, "saNtfAttributeChangeNotificationFilterAllocate failed - %s\n", error_output(errorCode));
return errorCode;
}
notificationFilterHandles.attributeChangeFilterHandle = attChFilter.notificationFilterHandle;
}
if (used_filters.all || used_filters.obj_cr_del) {
errorCode = saNtfObjectCreateDeleteNotificationFilterAllocate(ntfHandle,
&objCrDelFilter,
notificationFilterAllocationParams->numEventTypes,
notificationFilterAllocationParams->numNotificationObjects,
notificationFilterAllocationParams->numNotifyingObjects,
notificationFilterAllocationParams->numNotificationClassIds,
0);
if (errorCode != SA_AIS_OK) {
fprintf(stderr, "saNtfObjectCreateDeleteNotificationFilterAllocate failed - %s\n", error_output(errorCode));
return errorCode;
}
notificationFilterHandles.objectCreateDeleteFilterHandle = objCrDelFilter.notificationFilterHandle;
}
if (used_filters.all || used_filters.st_ch) {
errorCode = saNtfStateChangeNotificationFilterAllocate(ntfHandle,
&stChFilter,
notificationFilterAllocationParams->numEventTypes,
notificationFilterAllocationParams->numNotificationObjects,
notificationFilterAllocationParams->numNotifyingObjects,
notificationFilterAllocationParams->numNotificationClassIds,
0,
0);
if (errorCode != SA_AIS_OK) {
fprintf(stderr, "saNtfStateChangeNotificationFilterAllocate failed - %s\n", error_output(errorCode));
return errorCode;
}
notificationFilterHandles.stateChangeFilterHandle = stChFilter.notificationFilterHandle;
}
if (used_filters.all || used_filters.sec_al) {
errorCode = saNtfSecurityAlarmNotificationFilterAllocate(ntfHandle,
&secAlarmFilter,
notificationFilterAllocationParams->numEventTypes,
notificationFilterAllocationParams->numNotificationObjects,
notificationFilterAllocationParams->numNotifyingObjects,
notificationFilterAllocationParams->numNotificationClassIds,
0,0,0,0,0);
if (errorCode != SA_AIS_OK) {
fprintf(stderr, "saNtfSecurityAlarmNotificationFilterAllocate failed - %s\n", error_output(errorCode));
return errorCode;
}
notificationFilterHandles.securityAlarmFilterHandle = secAlarmFilter.notificationFilterHandle;
}
errorCode = saNtfNotificationSubscribe(¬ificationFilterHandles, subscriptionId);
if (SA_AIS_OK != errorCode) {
fprintf(stderr, "saNtfNotificationSubscribe failed - %s\n", error_output(errorCode));
return errorCode;
}
freeNtfFilter (¬ificationFilterHandles.alarmFilterHandle);
freeNtfFilter (¬ificationFilterHandles.attributeChangeFilterHandle);
freeNtfFilter (¬ificationFilterHandles.objectCreateDeleteFilterHandle);
freeNtfFilter (¬ificationFilterHandles.stateChangeFilterHandle);
freeNtfFilter (¬ificationFilterHandles.securityAlarmFilterHandle);
return errorCode;
}
void ahrs_propagate(float dt)
{
struct FloatVect3 accel;
struct FloatRates body_rates;
// realign all the filter if needed
// a complete init cycle is required
if (ins_impl.reset) {
ins_impl.reset = FALSE;
ins.status = INS_UNINIT;
ahrs.status = AHRS_UNINIT;
init_invariant_state();
}
// fill command vector
struct Int32Rates gyro_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro);
RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
struct Int32Vect3 accel_meas_body;
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);
// update correction gains
error_output(&ins_impl);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float *)&new_state,
(float *)&ins_impl.state, INV_STATE_DIM,
(float *)&ins_impl.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ins_impl.state = new_state;
// normalize quaternion
FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);
// set global state
stateSetNedToBodyQuat_f(&ins_impl.state.quat);
RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
stateSetBodyRates_f(&body_rates);
stateSetPositionNed_f(&ins_impl.state.pos);
stateSetSpeedNed_f(&ins_impl.state.speed);
// untilt accel and remove gravity
struct FloatQuat q_b2n;
float_quat_invert(&q_b2n, &ins_impl.state.quat);
float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel);
VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
VECT3_ADD(accel, A);
stateSetAccelNed_f((struct NedCoor_f *)&accel);
//------------------------------------------------------------//
#if SEND_INVARIANT_FILTER
struct FloatEulers eulers;
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
RunOnceEvery(3, {
pprz_msg_send_INV_FILTER(trans, dev, AC_ID,
&ins_impl.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&ins_impl.state.speed.x,
&ins_impl.state.speed.y,
&ins_impl.state.speed.z,
&ins_impl.state.pos.x,
&ins_impl.state.pos.y,
&ins_impl.state.pos.z,
&ins_impl.state.bias.p,
&ins_impl.state.bias.q,
&ins_impl.state.bias.r,
&ins_impl.state.as,
&ins_impl.state.hb,
&ins_impl.meas.baro_alt,
&ins_impl.meas.pos_gps.z)
});
#endif
#if LOG_INVARIANT_FILTER
if (pprzLogFile.fs != NULL) {
if (!log_started) {
// log file header
sdLogWriteLog(&pprzLogFile,
"p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
log_started = TRUE;
} else {
sdLogWriteLog(&pprzLogFile,
"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
ins_impl.cmd.rates.p,
ins_impl.cmd.rates.q,
ins_impl.cmd.rates.r,
ins_impl.cmd.accel.x,
ins_impl.cmd.accel.y,
ins_impl.cmd.accel.z,
ins_impl.meas.pos_gps.x,
ins_impl.meas.pos_gps.y,
ins_impl.meas.pos_gps.z,
ins_impl.meas.speed_gps.x,
ins_impl.meas.speed_gps.y,
ins_impl.meas.speed_gps.z,
ins_impl.meas.mag.x,
ins_impl.meas.mag.y,
ins_impl.meas.mag.z,
ins_impl.meas.baro_alt,
ins_impl.state.quat.qi,
ins_impl.state.quat.qx,
ins_impl.state.quat.qy,
ins_impl.state.quat.qz,
ins_impl.state.bias.p,
ins_impl.state.bias.q,
ins_impl.state.bias.r,
ins_impl.state.speed.x,
ins_impl.state.speed.y,
ins_impl.state.speed.z,
ins_impl.state.pos.x,
ins_impl.state.pos.y,
ins_impl.state.pos.z,
ins_impl.state.hb,
ins_impl.state.as);
}
}
#endif
}