void logging_navstate()
{
// return;
if ( !props_inited ) {
init_props();
}
if ( fnavstate == NULL ) {
return;
}
double pretty_yaw
= filter_psi_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
if ( pretty_yaw < 0 ) {
pretty_yaw += 2 * 3.14159265358979323846;
}
//struct timeval tv;
//gettimeofday( &tv, NULL );
//double unixSec = tv.tv_sec + (tv.tv_usec / 1000000.0);
fprintf( fnavstate,
"%.3f %.12f %.12f %.3f %.4f %.4f %.4f %.4f %.4f %.4f\n",
imu_timestamp_node->getDoubleValue() /* unixSec */,
filter_lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS,
filter_lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS,
-filter_alt_node->getDoubleValue(),
filter_vn_node->getDoubleValue(),
filter_ve_node->getDoubleValue(),
filter_vd_node->getDoubleValue(),
pretty_yaw,
filter_theta_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS,
filter_phi_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS
);
}
///===================================================================================== Archive
Archive::Archive(const Lib & lib, const ustring & path)
{
LogNoise(L"%p, '%s', 0x%Iu\n", &lib, path.c_str());
open_archive(lib, path);
init_props();
}
Archive::Archive(Com::Object<IInArchive> arc) :
m_arc(arc)
{
LogTrace();
init_props();
}
// periodic console summary of attitude/location estimate
void display_message()
{
if ( !props_inited ) {
init_props();
}
printf("[m/s^2]:ax = %6.3f ay = %6.3f az = %6.3f \n",
imu_ax_node->getDoubleValue(),
imu_ay_node->getDoubleValue(),
imu_az_node->getDoubleValue());
printf("[deg/s]:p = %6.3f q = %6.3f r = %6.3f \n",
imu_p_node->getDoubleValue() * SGD_RADIANS_TO_DEGREES,
imu_q_node->getDoubleValue() * SGD_RADIANS_TO_DEGREES,
imu_r_node->getDoubleValue() * SGD_RADIANS_TO_DEGREES);
printf("[Gauss]:hx = %6.3f hy = %6.3f hz = %6.3f \n",
imu_hx_node->getDoubleValue(),
imu_hy_node->getDoubleValue(),
imu_hz_node->getDoubleValue());
printf("[deg ]:phi = %6.2f the = %6.2f psi = %6.2f \n",
filter_phi_node->getDoubleValue(),
filter_theta_node->getDoubleValue(),
filter_psi_node->getDoubleValue());
printf("[ ]:Palt = %6.3f Pspd = %6.3f \n",
airdata_altitude_node->getDoubleValue(),
airdata_airspeed_node->getDoubleValue());
#if 0
// gyro bias from mnav filter
printf("[deg/s]:bp = %6.3f,bq = %6.3f,br = %6.3f \n",
xs[4] * SGD_RADIANS_TO_DEGREES,
xs[5] * SGD_RADIANS_TO_DEGREES,
xs[6] * SGD_RADIANS_TO_DEGREES);
#endif
if ( GPS_age() < 10.0 ) {
time_t current_time = gps_unix_sec_node->getIntValue();
double remainder = gps_unix_sec_node->getDoubleValue() - current_time;
struct tm *date = gmtime(¤t_time);
printf("[GPS ]:date = %04d/%02d/%02d %02d:%02d:%05.2f\n",
date->tm_year + 1900, date->tm_mon + 1, date->tm_mday,
date->tm_hour, date->tm_min, date->tm_sec + remainder);
printf("[GPS ]:lon = %f[deg], lat = %f[deg], alt = %f[m], age = %.2f\n",
gps_lon_node->getDoubleValue(), gps_lat_node->getDoubleValue(),
gps_alt_node->getDoubleValue(), GPS_age());
} else {
printf("[GPS ]:[%0f seconds old]\n", GPS_age());
}
if ( strcmp( filter_status_node->getStringValue(), "valid" ) == 0 ) {
printf("[filter]:lon = %f[deg], lat = %f[deg], alt = %f[m]\n",
filter_lon_node->getDoubleValue(),
filter_lat_node->getDoubleValue(),
filter_alt_node->getDoubleValue());
} else {
printf("[filter]:[No Valid Data]\n");
}
printf("[act ]: %.2f %.2f %.2f %.2f %.2f\n",
act_aileron_node->getDoubleValue(),
act_elevator_node->getDoubleValue(),
act_throttle_node->getDoubleValue(),
act_rudder_node->getDoubleValue(),
act_channel5_node->getDoubleValue());
printf("[health]: cmdseq = %d tgtwp = %d loadavg = %.2f\n",
link_seq_num->getIntValue(), target_waypoint->getIntValue(),
system_load_avg->getFloatValue());
printf("\n");
// printf("imu size = %d\n", sizeof( struct imu ) );
}
