/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include class ReplayVehicle { public: ReplayVehicle() { unused = -1; } void setup(); void load_parameters(void); AP_InertialSensor ins; AP_Baro barometer; AP_GPS gps; Compass compass; AP_SerialManager serial_manager; RangeFinder rng{serial_manager}; NavEKF2 EKF2{&ahrs, rng}; NavEKF3 EKF3{&ahrs, rng}; AP_AHRS_NavEKF ahrs{EKF2, EKF3}; AP_Vehicle::FixedWing aparm; AP_Airspeed airspeed; AP_Int32 unused; // logging is magic for Replay; this is unused struct LogStructure log_structure[256] = { }; AP_Logger logger{unused}; private: Parameters g; // setup the var_info table AP_Param param_loader{var_info}; static const AP_Param::Info var_info[]; }; class Replay : public AP_HAL::HAL::Callbacks { public: Replay(ReplayVehicle &vehicle) : filename("log.bin"), _vehicle(vehicle) { } // HAL::Callbacks implementation. void setup() override; void loop() override; void flush_logger(void); void show_packet_counts(); bool check_solution = false; const char *log_filename = NULL; bool generate_fpe = true; /* information about a log from find_log_info */ struct log_information { uint16_t update_rate; bool have_imu2:1; bool have_imt:1; bool have_imt2:1; } log_info {}; // return true if a user parameter of name is set bool check_user_param(const char *name); private: const char *filename; ReplayVehicle &_vehicle; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL sitl; #endif LogReader logreader{_vehicle.ahrs, _vehicle.ins, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.logger, _vehicle.log_structure, 0, nottypes}; FILE *ekf1f; FILE *ekf2f; FILE *ekf3f; FILE *ekf4f; bool done_baro_init; bool done_home_init; int32_t arm_time_ms = -1; bool ahrs_healthy; bool use_imt = true; bool check_generate = false; float tolerance_euler = 3; float tolerance_pos = 2; float tolerance_vel = 2; const char **nottypes = NULL; uint16_t downsample = 0; bool logmatch = false; uint32_t output_counter = 0; uint64_t last_timestamp = 0; bool packet_counts = false; struct { float max_roll_error; float max_pitch_error; float max_yaw_error; float max_pos_error; float max_alt_error; float max_vel_error; } check_result {}; void _parse_command_line(uint8_t argc, char * const argv[]); struct user_parameter { struct user_parameter *next; char name[17]; float value; } *user_parameters; void set_ins_update_rate(uint16_t update_rate); void inhibit_gyro_cal(); void force_log_disarmed(); void usage(void); void set_user_parameters(void); void read_sensors(const char *type); void write_ekf_logs(void); void log_check_generate(); void log_check_solution(); bool show_error(const char *text, float max_error, float tolerance); void report_checks(); bool find_log_info(struct log_information &info); const char **parse_list_from_string(const char *str); bool parse_param_line(char *line, char **vname, float &value); void load_param_file(const char *filename); void set_signal_handlers(void); void flush_and_exit(); FILE *xfopen(const char *f, const char *mode); bool seen_non_fmt; }; /* Replay specific log structures */ struct PACKED log_Chek { LOG_PACKET_HEADER; uint64_t time_us; int16_t roll; int16_t pitch; uint16_t yaw; int32_t lat; int32_t lng; float alt; float vnorth; float veast; float vdown; }; extern Replay replay;