void Replay::setup() { ::printf("Starting\n"); uint8_t argc; char * const *argv; hal.util->commandline_arguments(argc, argv); _parse_command_line(argc, argv); if (!check_generate) { logreader.set_save_chek_messages(true); } // _parse_command_line sets up an FPE handler. We can do better: signal(SIGFPE, _replay_sig_fpe); hal.console->printf("Processing log %s\n", filename); // remember filename for reporting log_filename = filename; if (!find_log_info(log_info)) { printf("Update to get log information\n"); exit(1); } hal.console->printf("Using an update rate of %u Hz\n", log_info.update_rate); if (!logreader.open_log(filename)) { perror(filename); exit(1); } _vehicle.setup(); inhibit_gyro_cal(); set_ins_update_rate(log_info.update_rate); feenableexcept(FE_INVALID | FE_OVERFLOW); plotf = fopen("plot.dat", "w"); plotf2 = fopen("plot2.dat", "w"); ekf1f = fopen("EKF1.dat", "w"); ekf2f = fopen("EKF2.dat", "w"); ekf3f = fopen("EKF3.dat", "w"); ekf4f = fopen("EKF4.dat", "w"); fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n"); fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n"); fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n"); fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n"); fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n"); }
void Replay::setup() { ::printf("Starting\n"); uint8_t argc; char * const *argv; hal.util->commandline_arguments(argc, argv); _parse_command_line(argc, argv); if (!check_generate) { logreader.set_save_chek_messages(true); } set_signal_handlers(); hal.console->printf("Processing log %s\n", filename); // remember filename for reporting log_filename = filename; if (!find_log_info(log_info)) { printf("Update to get log information\n"); exit(1); } hal.console->printf("Using an update rate of %u Hz\n", log_info.update_rate); if (!logreader.open_log(filename)) { perror(filename); exit(1); } _vehicle.setup(); inhibit_gyro_cal(); if (log_info.update_rate == 400) { // assume copter for 400Hz _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); _vehicle.ahrs.set_fly_forward(false); } else if (log_info.update_rate == 50) { // assume copter for 400Hz _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); _vehicle.ahrs.set_fly_forward(true); } set_ins_update_rate(log_info.update_rate); }