static void
xsens_mti_message_handler(carmen_xsens_global_quat_message *xsens_mti)
{
if (!xsens_handler.initial_state_initialized)
{
initialize_states(xsens_mti);
return;
}
// Must check if timestamp has changed due to log file jump on log playback
if (reinitialized_gps)//if (check_time_difference(xsens_mti->timestamp))
{
carmen_fused_odometry_initialize(fused_odometry_parameters);
initialize_states(xsens_mti);
reinitialized_gps = 0;
return;
}
xsens_sensor_vector_index = (xsens_sensor_vector_index + 1) % XSENS_SENSOR_VECTOR_SIZE;
if (xsens_sensor_vector[xsens_sensor_vector_index] != NULL)
destroy_sensor_vector_xsens_xyz(xsens_sensor_vector[xsens_sensor_vector_index]);
sensor_vector_xsens_xyz *sensor_vector = create_sensor_vector_xsens_mti(xsens_mti, &xsens_handler.gps_xyz);
xsens_sensor_vector[xsens_sensor_vector_index] = sensor_vector;
set_best_yaw_estimate(xsens_sensor_vector, xsens_sensor_vector_index);
xsens_handler.orientation = sensor_vector->orientation; // Used by create_car_odometry_control()
xsens_handler.ang_velocity = sensor_vector->ang_velocity; // Used by create_car_odometry_control()
if (!kalman_filter)
{
if (xsens_handler.gps_performance_changed)
{
xsens_handler.gps_performance_degradation = 50.0;
xsens_handler.gps_performance_changed = 0;
}
if (xsens_handler.gps_orientation_performance_changed)
{
xsens_handler.gps_orientation_performance_degradation = 40.0;
xsens_handler.gps_orientation_performance_changed = 0;
}
prediction(sensor_vector->timestamp, fused_odometry_parameters);
correction(measure_weight_orientation, sensor_vector, fused_odometry_parameters);
publish_fused_odometry();
xsens_handler.gps_performance_degradation *= 0.98; // fator de decaimento
xsens_handler.gps_orientation_performance_degradation *= 0.98; // fator de decaimento
}
xsens_handler.last_xsens_message_timestamp = xsens_mti->timestamp;
}
int
main(int argc, char** argv)
{
argc_g = argc;
argv_g = argv;
carmen_fused_odometry_parameters fused_odometry_parameters;
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
initialize_carmen_parameters(argc, argv, &fused_odometry_parameters);
register_ipc_messages();
carmen_fused_odometry_initialize(&fused_odometry_parameters);
carmen_ipc_dispatch();
return 0;
}
int
main(int argc, char** argv)
{
argc_g = argc;
argv_g = argv;
carmen_fused_odometry_parameters fused_odometry_parameters;
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
initialize_carmen_parameters(argc, argv, &fused_odometry_parameters);
register_ipc_messages();
carmen_fused_odometry_initialize(&fused_odometry_parameters);
// carmen_ipc_addPeriodicTimer(1.0 / fused_odometry_frequency, (TIMER_HANDLER_TYPE) correct_fused_odometry_timming_and_publish, NULL);
carmen_ipc_dispatch();
// fused_odometry_main_loop();
return 0;
}