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();
}
}
}
