Line data    Source code

       1             : #include "AP_Filter_config.h"
       2             : 
       3             : #if AP_FILTER_ENABLED
       4             : 
       5             : #include "AP_Filter.h"
       6             : #include <AP_BoardConfig/AP_BoardConfig.h>
       7             : #include <AP_Vehicle/AP_Vehicle_Type.h>
       8             : #include <AP_HAL/AP_HAL_Boards.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12             : const AP_Param::GroupInfo AP_Filters::var_info[] = {
      13             : 
      14             : #if AP_FILTER_NUM_FILTERS >= 1
      15             :     // @Group: 1_
      16             :     // @Path: AP_Filter_params.cpp
      17             :     AP_SUBGROUPINFO(params[0], "1_", 2, AP_Filters, AP_Filter_params),
      18             :     // @Group: 1_
      19             :     // @Path: AP_NotchFilter_params.cpp
      20             :     AP_SUBGROUPVARPTR(filters[0], "1_", 3, AP_Filters, backend_var_info[0]),
      21             : #endif
      22             : #if AP_FILTER_NUM_FILTERS >= 2
      23             :     // @Group: 2_
      24             :     // @Path: AP_Filter_params.cpp
      25             :     AP_SUBGROUPINFO(params[1], "2_", 4, AP_Filters, AP_Filter_params),
      26             :     // @Group: 2_
      27             :     // @Path: AP_NotchFilter_params.cpp
      28             :     AP_SUBGROUPVARPTR(filters[1], "2_", 5, AP_Filters, backend_var_info[1]),
      29             : #endif
      30             : #if AP_FILTER_NUM_FILTERS >= 3
      31             :     // @Group: 3_
      32             :     // @Path: AP_Filter_params.cpp
      33             :     AP_SUBGROUPINFO(params[2], "3_", 6, AP_Filters, AP_Filter_params),
      34             :     // @Group: 3_
      35             :     // @Path: AP_NotchFilter_params.cpp
      36             :     AP_SUBGROUPVARPTR(filters[2], "3_", 7, AP_Filters, backend_var_info[2]),
      37             : #endif
      38             : #if AP_FILTER_NUM_FILTERS >= 4
      39             :     // @Group: 4_
      40             :     // @Path: AP_Filter_params.cpp
      41             :     AP_SUBGROUPINFO(params[3], "4_", 8, AP_Filters, AP_Filter_params),
      42             :     // @Group: 4_
      43             :     // @Path: AP_NotchFilter_params.cpp
      44             :     AP_SUBGROUPVARPTR(filters[3], "4_", 9, AP_Filters, backend_var_info[3]),
      45             : #endif
      46             : #if AP_FILTER_NUM_FILTERS >= 5
      47             :     // @Group: 5_
      48             :     // @Path: AP_Filter_params.cpp
      49             :     AP_SUBGROUPINFO(params[4], "5_", 10, AP_Filters, AP_Filter_params),
      50             :     // @Group: 5_
      51             :     // @Path: AP_NotchFilter_params.cpp
      52             :     AP_SUBGROUPVARPTR(filters[4], "5_", 11, AP_Filters, backend_var_info[4]),
      53             : #endif
      54             : #if AP_FILTER_NUM_FILTERS >= 6
      55             :     // @Group: 6_
      56             :     // @Path: AP_Filter_params.cpp
      57             :     AP_SUBGROUPINFO(params[5], "6_", 12, AP_Filters, AP_Filter_params),
      58             :     // @Group: 6_
      59             :     // @Path: AP_NotchFilter_params.cpp
      60             :     AP_SUBGROUPVARPTR(filters[5], "6_", 13, AP_Filters, backend_var_info[5]),
      61             : #endif
      62             : #if AP_FILTER_NUM_FILTERS >= 7
      63             :     // @Group: 7_
      64             :     // @Path: AP_Filter_params.cpp
      65             :     AP_SUBGROUPINFO(params[6], "7_", 14, AP_Filters, AP_Filter_params),
      66             :     // @Group: 7_
      67             :     // @Path: AP_NotchFilter_params.cpp
      68             :     AP_SUBGROUPVARPTR(filters[6], "7_", 15, AP_Filters, backend_var_info[6]),
      69             : #endif
      70             : #if AP_FILTER_NUM_FILTERS >= 8
      71             :     // @Group: 8_
      72             :     // @Path: AP_Filter_params.cpp
      73             :     AP_SUBGROUPINFO(params[7], "8_", 16, AP_Filters, AP_Filter_params),
      74             :     // @Group: 8_
      75             :     // @Path: AP_NotchFilter_params.cpp
      76             :     AP_SUBGROUPVARPTR(filters[7], "8_", 17, AP_Filters, backend_var_info[7]),
      77             : #endif
      78             :     AP_GROUPEND
      79             : };
      80             : 
      81             : const AP_Param::GroupInfo *AP_Filters::backend_var_info[AP_FILTER_NUM_FILTERS];
      82             : 
      83          17 : AP_Filter::AP_Filter(FilterType type):
      84          17 :     _type(type)
      85             : {
      86          17 : }
      87             : 
      88       18882 : AP_Filters::AP_Filters()
      89             : {
      90        2098 :     AP_Param::setup_object_defaults(this, var_info);
      91             : 
      92             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      93        2096 :     if (singleton != nullptr) {
      94           0 :         AP_HAL::panic("AP_Filters must be singleton");
      95             :     }
      96             : #endif
      97        2098 :     singleton = this;
      98        2098 : }
      99             : 
     100        2086 : void AP_Filters::init()
     101             : {
     102        2086 :     update();
     103        2086 : }
     104             : 
     105             : // 1Hz update to process config changes
     106      151303 : void AP_Filters::update()
     107             : {
     108      151303 :     if (hal.util->get_soft_armed()) {
     109      100449 :         return;
     110             :     }
     111             : 
     112             :     // make sure all filters are allocated
     113      457686 :     for (uint8_t i = 0; i < AP_FILTER_NUM_FILTERS; i++) {
     114      406832 :         bool update = false;
     115      406832 :         switch (AP_Filter::FilterType(params[i]._type)) {
     116      406732 :             case AP_Filter::FilterType::FILTER_NONE:
     117      406732 :                 break;
     118         100 :             case AP_Filter::FilterType::FILTER_NOTCH:
     119         100 :                 if (filters[i] == nullptr) {
     120          17 :                     filters[i] = NEW_NOTHROW AP_NotchFilter_params();
     121          17 :                     backend_var_info[i] = AP_NotchFilter_params::var_info;
     122          17 :                     update = true;
     123             :                 }
     124         100 :                 break;
     125           0 :             default:
     126           0 :                 return;
     127             :         }
     128             : 
     129      406832 :         if (update) {
     130          17 :             AP_Param::load_object_from_eeprom(filters[i], backend_var_info[i]);
     131          17 :             AP_Param::invalidate_count();
     132             :         }
     133             :     }
     134             : }
     135             : 
     136         723 : AP_Filter* AP_Filters::get_filter(uint8_t index)
     137             : {
     138         723 :     if (index >= AP_FILTER_NUM_FILTERS) {
     139           0 :         return nullptr;
     140             :     }
     141             : 
     142         723 :     return filters[index-1];
     143             : }
     144             : 
     145             : // singleton instance
     146             : AP_Filters *AP_Filters::singleton;
     147             : 
     148             : namespace AP {
     149             : 
     150         723 : AP_Filters &filters()
     151             : {
     152         723 :     return *AP_Filters::get_singleton();
     153             : }
     154             : 
     155             : }
     156             : 
     157             : #endif // AP_FILTER_ENABLED