ardupilot/Tools/Replay/Replay.h

/*
   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 <http://www.gnu.org/licenses/>.
 */


#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h>
#include <fenv.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_AccelCal/AP_AccelCal.h>
#include <AP_ADC/AP_ADC.h>
#include <AP_Declination/AP_Declination.h>
#include <Filter/Filter.h>
#include <AP_Buffer/AP_Buffer.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Notify/AP_Notify.h>
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_InertialNav/AP_InertialNav.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <stdio.h>
#include <errno.h>
#include <signal.h>
#include <unistd.h>
#include <AP_HAL/utility/getopt_cpp.h>

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, ROTATION_PITCH_270};
    NavEKF2 EKF2{&ahrs, barometer, rng};
    NavEKF3 EKF3{&ahrs, barometer, rng};
    AP_AHRS_NavEKF ahrs {ins, barometer, gps, rng, EKF2, EKF3};
    AP_InertialNav_NavEKF inertial_nav{ahrs};
    AP_Vehicle::FixedWing aparm;
    AP_Airspeed airspeed;
    AP_Int32 unused; // logging is magic for Replay; this is unused
    DataFlash_Class dataflash{"Replay v0.1", 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_dataflash(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.barometer, _vehicle.compass, _vehicle.gps, _vehicle.airspeed, _vehicle.dataflash, nottypes};

    FILE *ekf1f;
    FILE *ekf2f;
    FILE *ekf3f;
    FILE *ekf4f;

    bool done_parameters;
    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 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);
};

enum {
    LOG_CHEK_MSG=100
};

/*
  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;