Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "quadplane.h"
       4             : 
       5             : #ifndef AP_PLANE_SYSTEMID_ENABLED
       6             : // enable via custom build server
       7             : #define AP_PLANE_SYSTEMID_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_QUADPLANE_ENABLED
       8             : #endif
       9             : 
      10             : #if AP_PLANE_SYSTEMID_ENABLED
      11             : 
      12             : #include <AP_Math/chirp.h>
      13             : #include <AP_Param/AP_Param.h>
      14             : #include <AP_Math/vector3.h>
      15             : #include <AP_Math/vector2.h>
      16             : 
      17             : class AP_SystemID {
      18             : 
      19             : public:
      20             :     AP_SystemID(void);
      21             :     void start(void);
      22             :     void stop(void);
      23             :     void fw_update();
      24             :     void vtol_update();
      25             : 
      26             :     static const struct AP_Param::GroupInfo var_info[];
      27             : 
      28      810697 :     const Vector3f &get_attitude_offset_deg(void) const {
      29      810697 :         return attitude_offset_deg;
      30             :     }
      31           0 :     const Vector3f &get_output_offset(void) const {
      32           0 :         return output_offset;
      33             :     }
      34      213454 :     float get_throttle_offset(void) const {
      35      213454 :         return running ? throttle_offset : 0.0;
      36             :     }
      37             : 
      38    26123248 :     bool is_running() const {
      39    26123248 :         return running;
      40             :     }
      41             : 
      42             :     // Return true if a fixed wing system ID is currently running
      43             :     bool is_running_fw() const;
      44             : 
      45             : private:
      46             :     Chirp chirp_input;
      47             :     bool running;
      48             :     // AxisType 14-19 not supported for plane or quadplane
      49             :     enum class AxisType {
      50             :         NONE = 0,               // none
      51             :         INPUT_ROLL = 1,         // angle input roll axis is being excited
      52             :         INPUT_PITCH = 2,        // angle pitch axis is being excited
      53             :         INPUT_YAW = 3,          // angle yaw axis is being excited
      54             :         RECOVER_ROLL = 4,       // angle roll axis is being excited
      55             :         RECOVER_PITCH = 5,      // angle pitch axis is being excited
      56             :         RECOVER_YAW = 6,        // angle yaw axis is being excited
      57             :         RATE_ROLL = 7,          // rate roll axis is being excited
      58             :         RATE_PITCH = 8,         // rate pitch axis is being excited
      59             :         RATE_YAW = 9,           // rate yaw axis is being excited
      60             :         MIX_ROLL = 10,          // mixer roll axis is being excited
      61             :         MIX_PITCH = 11,         // mixer pitch axis is being excited
      62             :         MIX_YAW = 12,           // mixer pitch axis is being excited
      63             :         MIX_THROTTLE = 13,      // mixer throttle axis is being excited
      64             :         FW_INPUT_ROLL = 20,     // fixed wing angle input roll axis is being excited
      65             :         FW_INPUT_PITCH = 21,    // fixed wing angle input pitch axis is being excited
      66             :         FW_MIX_ROLL = 22,       // fixed wing mixer roll axis is being excited
      67             :         FW_MIX_PITCH = 23,      // fixed wing mixer pitch axis is being excited
      68             :     };
      69             : 
      70             :     void set_bf_feedforward(bool value);
      71             :     void log_data() const;
      72             :     void log_plane_data() const;
      73             :     int8_t log_subsample;       // Subsample multiple for logging.
      74             : 
      75             : 
      76             :     AP_Enum<AxisType> axis;               // Controls which axis are being excited. Set to non-zero to display other parameters
      77             :     AP_Float waveform_magnitude;// Magnitude of chirp waveform
      78             :     AP_Float frequency_start;   // Frequency at the start of the chirp
      79             :     AP_Float frequency_stop;    // Frequency at the end of the chirp
      80             :     AP_Float time_fade_in;      // Time to reach maximum amplitude of chirp
      81             :     AP_Float time_record;       // Time taken to complete the chirp waveform
      82             :     AP_Float time_fade_out;     // Time to reach zero amplitude after chirp finishes
      83             :     AP_Float xy_control_mul;    // multiplier for VTOL XY control
      84             : 
      85             :     struct {
      86             :         bool att_bf_feedforward;    // Setting of attitude_control->get_bf_feedforward
      87             :     } restore;
      88             : 
      89             :     float waveform_time;        // Time reference for waveform
      90             :     float waveform_sample;      // Current waveform sample
      91             :     float waveform_freq_rads;   // Instantaneous waveform frequency
      92             :     float time_const_freq;      // Time at constant frequency before chirp starts
      93             :     uint32_t last_loop_time_ms;   // time in milliseconds of last loop
      94             : 
      95             :     // current attitude offset
      96             :     Vector3f attitude_offset_deg;
      97             :     Vector3f output_offset;
      98             :     float throttle_offset;
      99             : 
     100             :     AxisType start_axis;
     101             : 
     102             : };
     103             : 
     104             : #endif // AP_PLANE_SYSTEMID_ENABLED