Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :    This program is free software: you can redistribute it and/or modify
       5             :    it under the terms of the GNU General Public License as published by
       6             :    the Free Software Foundation, either version 3 of the License, or
       7             :    (at your option) any later version.
       8             : 
       9             :    This program is distributed in the hope that it will be useful,
      10             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :    GNU General Public License for more details.
      13             : 
      14             :    You should have received a copy of the GNU General Public License
      15             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : 
      18             : #pragma once
      19             : 
      20             : #include "AP_GPS_config.h"
      21             : 
      22             : #if AP_GPS_BLENDED_ENABLED
      23             : 
      24             : #include "AP_GPS.h"
      25             : #include "GPS_Backend.h"
      26             : 
      27             : class AP_GPS_Blended : public AP_GPS_Backend
      28             : {
      29             : public:
      30             : 
      31        2092 :     AP_GPS_Blended(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, class AP_GPS::GPS_timing &_timing) :
      32             :         AP_GPS_Backend(_gps, _params, _state, nullptr),
      33        2092 :         timing{_timing}
      34        2092 :         { }
      35             : 
      36             :     // pre-arm check of GPS blending.  False if blending is unhealthy,
      37             :     // True if healthy or blending is not being used
      38       40189 :     bool is_healthy() const override {
      39       40189 :         return (_blend_health_counter < 50);
      40             :     }
      41             : 
      42           0 :     bool read() override { return true; }
      43             : 
      44           0 :     const char *name() const override { return "Blended"; }
      45             : 
      46             :     bool get_lag(float &lag_sec) const override;
      47    41156285 :     const Vector3f &get_antenna_offset() const {
      48    41156285 :         return _blended_antenna_offset;
      49             :     }
      50             : 
      51             :     // calculate the blend weight.  Returns true if blend could be
      52             :     // calculated, false if not
      53             :     bool calc_weights(void);
      54             :     // calculate the blended state
      55             :     void calc_state(void);
      56             : 
      57     7438667 :     void zero_health_counter() {
      58     7438667 :         _blend_health_counter = 0;
      59     7438667 :     }
      60             : 
      61             : private:
      62             : 
      63             :     // GPS blending and switching
      64             :     Vector3f _blended_antenna_offset; // blended antenna offset
      65             :     float _blended_lag_sec; // blended receiver lag in seconds
      66             :     float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
      67             :     uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy
      68             : 
      69             :     AP_GPS::GPS_timing &timing;
      70             :     bool _calc_weights(void);
      71             : };
      72             : 
      73             : #endif  // AP_GPS_BLENDED_ENABLED