void Replay::read_sensors(const char *type) { if (!done_parameters && !streq(type,"FMT") && !streq(type,"PARM")) { done_parameters = true; set_user_parameters(); } if (streq(type,"IMU2")) { have_imu2 = true; } if (use_imt && streq(type,"IMT")) { have_imt = true; } if (use_imt && streq(type,"IMT2")) { have_imt2 = true; } if (streq(type,"GPS")) { _vehicle.gps.update(); if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) { _vehicle.ahrs.estimate_wind(); } } else if (streq(type,"MAG")) { _vehicle.compass.read(); } else if (streq(type,"ARSP")) { _vehicle.ahrs.set_airspeed(&_vehicle.airspeed); } else if (streq(type,"BARO")) { _vehicle.barometer.update(); if (!done_baro_init) { done_baro_init = true; ::printf("Barometer initialised\n"); _vehicle.barometer.update_calibration(); } } bool run_ahrs = false; if (streq(type,"FRAM")) { if (!have_fram) { have_fram = true; printf("Have FRAM framing\n"); } run_ahrs = true; } if (have_imt) { if ((streq(type,"IMT") && !have_imt2) || (streq(type,"IMT2") && have_imt2)) { run_ahrs = true; } } // special handling of IMU messages as these trigger an ahrs.update() if (!have_fram && !have_imt && ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) { run_ahrs = true; } /* always run AHRS on CHECK messages when checking the solution */ if (check_solution) { run_ahrs = streq(type, "CHEK"); } if (run_ahrs) { _vehicle.ahrs.update(); if (_vehicle.ahrs.get_home().lat != 0) { _vehicle.inertial_nav.update(_vehicle.ins.get_delta_time()); } _vehicle.dataflash.Log_Write_EKF(_vehicle.ahrs,false); _vehicle.dataflash.Log_Write_AHRS2(_vehicle.ahrs); _vehicle.dataflash.Log_Write_POS(_vehicle.ahrs); if (_vehicle.ahrs.healthy() != ahrs_healthy) { ahrs_healthy = _vehicle.ahrs.healthy(); printf("AHRS health: %u at %lu\n", (unsigned)ahrs_healthy, (unsigned long)hal.scheduler->millis()); } if (check_generate) { log_check_generate(); } else if (check_solution) { log_check_solution(); } } }
void Replay::read_sensors(const char *type) { if (!done_parameters && !streq(type,"FMT") && !streq(type,"PARM")) { done_parameters = true; set_user_parameters(); } if (done_parameters && streq(type, "PARM")) { set_user_parameters(); } if (!done_home_init) { if (streq(type, "GPS") && (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) { const Location &loc = _vehicle.gps.location(); ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", loc.lat * 1.0e-7f, loc.lng * 1.0e-7f, loc.alt * 0.01f, AP_HAL::millis()*0.001f); _vehicle.ahrs.set_home(loc); _vehicle.compass.set_initial_location(loc.lat, loc.lng); done_home_init = true; } } if (streq(type,"GPS")) { _vehicle.gps.update(); if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) { _vehicle.ahrs.estimate_wind(); } } else if (streq(type,"MAG")) { _vehicle.compass.read(); } else if (streq(type,"ARSP")) { _vehicle.ahrs.set_airspeed(&_vehicle.airspeed); } else if (streq(type,"BARO")) { _vehicle.barometer.update(); if (!done_baro_init) { done_baro_init = true; ::printf("Barometer initialised\n"); _vehicle.barometer.update_calibration(); } } bool run_ahrs = false; if (log_info.have_imt2) { run_ahrs = streq(type, "IMT2"); _vehicle.ahrs.force_ekf_start(); } else if (log_info.have_imt) { run_ahrs = streq(type, "IMT"); _vehicle.ahrs.force_ekf_start(); } else if (log_info.have_imu2) { run_ahrs = streq(type, "IMU2"); } else { run_ahrs = streq(type, "IMU"); } /* always run AHRS on CHECK messages when checking the solution */ if (check_solution) { run_ahrs = streq(type, "CHEK"); } if (run_ahrs) { _vehicle.ahrs.update(); if (_vehicle.ahrs.get_home().lat != 0) { _vehicle.inertial_nav.update(_vehicle.ins.get_delta_time()); } if ((downsample == 0 || ++output_counter % downsample == 0) && !logmatch) { write_ekf_logs(); } if (_vehicle.ahrs.healthy() != ahrs_healthy) { ahrs_healthy = _vehicle.ahrs.healthy(); printf("AHRS health: %u at %lu\n", (unsigned)ahrs_healthy, (unsigned long)AP_HAL::millis()); } if (check_generate) { log_check_generate(); } else if (check_solution) { log_check_solution(); } } if (logmatch && streq(type, "NKF1")) { write_ekf_logs(); } }
void Replay::read_sensors(const char *type) { if (!done_parameters && !streq(type,"FMT") && !streq(type,"PARM")) { done_parameters = true; set_user_parameters(); } if (use_imt && streq(type,"IMT")) { have_imt = true; } if (use_imt && streq(type,"IMT2")) { have_imt2 = true; } if (!done_home_init) { if (streq(type, "GPS") && (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) { const Location &loc = _vehicle.gps.location(); ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", loc.lat * 1.0e-7f, loc.lng * 1.0e-7f, loc.alt * 0.01f, hal.scheduler->millis()*0.001f); _vehicle.ahrs.set_home(loc); _vehicle.compass.set_initial_location(loc.lat, loc.lng); done_home_init = true; } } if (streq(type,"GPS")) { _vehicle.gps.update(); if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) { _vehicle.ahrs.estimate_wind(); } } else if (streq(type,"MAG")) { _vehicle.compass.read(); } else if (streq(type,"ARSP")) { _vehicle.ahrs.set_airspeed(&_vehicle.airspeed); } else if (streq(type,"BARO")) { _vehicle.barometer.update(); if (!done_baro_init) { done_baro_init = true; ::printf("Barometer initialised\n"); _vehicle.barometer.update_calibration(); } } bool run_ahrs = false; if (streq(type,"FRAM")) { if (!have_fram) { have_fram = true; printf("Have FRAM framing\n"); } run_ahrs = true; } if (have_imt) { if ((streq(type,"IMT") && !have_imt2) || (streq(type,"IMT2") && have_imt2)) { run_ahrs = true; } } // special handling of IMU messages as these trigger an ahrs.update() if (!have_fram && !have_imt && ((streq(type,"IMU") && !log_info.have_imu2) || (streq(type, "IMU2") && log_info.have_imu2))) { run_ahrs = true; } /* always run AHRS on CHECK messages when checking the solution */ if (check_solution) { run_ahrs = streq(type, "CHEK"); } if (run_ahrs) { _vehicle.ahrs.update(); if (_vehicle.ahrs.get_home().lat != 0) { _vehicle.inertial_nav.update(_vehicle.ins.get_delta_time()); } if (downsample == 0 || ++output_counter % downsample == 0) { if (!LogReader::in_list("EKF", nottypes)) { _vehicle.dataflash.Log_Write_EKF(_vehicle.ahrs,false); } if (!LogReader::in_list("AHRS2", nottypes)) { _vehicle.dataflash.Log_Write_AHRS2(_vehicle.ahrs); } if (!LogReader::in_list("POS", nottypes)) { _vehicle.dataflash.Log_Write_POS(_vehicle.ahrs); } } if (_vehicle.ahrs.healthy() != ahrs_healthy) { ahrs_healthy = _vehicle.ahrs.healthy(); printf("AHRS health: %u at %lu\n", (unsigned)ahrs_healthy, (unsigned long)hal.scheduler->millis()); } if (check_generate) { log_check_generate(); } else if (check_solution) { log_check_solution(); } } }