Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :   Compass driver backend class. Each supported compass sensor type
      18             :   needs to have an object derived from this class.
      19             :  */
      20             : #pragma once
      21             : 
      22             : #include "AP_Compass_config.h"
      23             : 
      24             : #if AP_COMPASS_EXTERNALAHRS_ENABLED
      25             : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
      26             : #endif
      27             : 
      28             : #if AP_COMPASS_MSP_ENABLED
      29             : #include <AP_MSP/msp.h>
      30             : #endif
      31             : 
      32             : #include <AP_Math/AP_Math.h>
      33             : 
      34             : class Compass;  // forward declaration
      35             : class AP_Compass_Backend
      36             : {
      37             : public:
      38             :     AP_Compass_Backend();
      39             : 
      40             :     // we declare a virtual destructor so that drivers can
      41             :     // override with a custom destructor if need be.
      42          62 :     virtual ~AP_Compass_Backend(void) {}
      43             : 
      44             :     // read sensor data
      45     4524327 :     virtual void read(void) {
      46     4524327 :         drain_accumulated_samples();
      47     4524327 :     }
      48             : 
      49             :     /*
      50             :       device driver IDs. These are used to fill in the devtype field
      51             :       of the device ID, which shows up as COMPASS*ID* parameters to
      52             :       users. The values are chosen for compatibility with existing PX4
      53             :       drivers.
      54             :       If a change is made to a driver that would make existing
      55             :       calibration values invalid then this number must be changed.
      56             :      */
      57             :     enum DevTypes {
      58             :         DEVTYPE_HMC5883_OLD = 0x01,
      59             :         DEVTYPE_HMC5883 = 0x07,
      60             :         DEVTYPE_LSM303D = 0x02,
      61             :         DEVTYPE_AK8963  = 0x04,
      62             :         DEVTYPE_BMM150  = 0x05,
      63             :         DEVTYPE_LSM9DS1 = 0x06,
      64             :         DEVTYPE_LIS3MDL = 0x08,
      65             :         DEVTYPE_AK09916 = 0x09,
      66             :         DEVTYPE_IST8310 = 0x0A,
      67             :         DEVTYPE_ICM20948 = 0x0B,
      68             :         DEVTYPE_MMC3416 = 0x0C,
      69             :         DEVTYPE_QMC5883L = 0x0D,
      70             :         DEVTYPE_MAG3110  = 0x0E,
      71             :         DEVTYPE_SITL  = 0x0F,
      72             :         DEVTYPE_IST8308 = 0x10,
      73             :         DEVTYPE_RM3100 = 0x11,
      74             :         DEVTYPE_RM3100_2 = 0x12, // unused, past mistake
      75             :         DEVTYPE_MMC5983 = 0x13,
      76             :         DEVTYPE_AK09918 = 0x14,
      77             :         DEVTYPE_AK09915 = 0x15,
      78             :         DEVTYPE_QMC5883P = 0x16,
      79             :         DEVTYPE_BMM350 = 0x17,
      80             :         DEVTYPE_IIS2MDC = 0x18,
      81             :         // DEVTYPE_LIS2MDL = 0x19,  // DO NOT re-use this ID; same sensor as IIS2MDC
      82             :         DEVTYPE_AF9838 = 0x1A,
      83             :     };
      84             : 
      85             : #if AP_COMPASS_MSP_ENABLED
      86           0 :     virtual void handle_msp(const MSP::msp_compass_data_message_t &pkt) {}
      87             : #endif
      88             : 
      89             : #if AP_COMPASS_EXTERNALAHRS_ENABLED
      90      403275 :     virtual void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) {}
      91             : #endif
      92             :     
      93             : protected:
      94             : 
      95             :     // this backend's instance number
      96             :     uint8_t instance;
      97             : 
      98             :     /*
      99             :      * A compass measurement is expected to pass through the following functions:
     100             :      * 1. rotate_field - this rotates the measurement in-place from sensor frame
     101             :      *      to body frame
     102             :      * 2. publish_raw_field - this provides an uncorrected point-sample for
     103             :      *      calibration libraries
     104             :      * 3. correct_field - this corrects the measurement in-place for hard iron,
     105             :      *      soft iron, motor interference, and non-orthogonality errors
     106             :      * 4. publish_filtered_field - legacy filtered magnetic field
     107             :      *
     108             :      * All those functions expect the mag field to be in milligauss.
     109             :      */
     110             : 
     111             :     void rotate_field(Vector3f &mag);
     112             :     void publish_raw_field(const Vector3f &mag);
     113             :     void correct_field(Vector3f &mag);
     114             :     void publish_filtered_field(const Vector3f &mag);
     115             :     void set_last_update_usec(uint32_t last_update);
     116             : 
     117             :     void accumulate_sample(Vector3f &field,
     118             :                            uint32_t max_samples = 10);
     119             :     void drain_accumulated_samples(const Vector3f *scale = NULL);
     120             : 
     121             :     // register compass instance with the frontend
     122             :     bool register_compass(int32_t dev_id) WARN_IF_UNUSED;
     123             : 
     124             :     // set dev_id for an instance
     125             :     void set_dev_id(uint32_t dev_id);
     126             : 
     127             :     // save dev_id, used by SITL
     128             :     void save_dev_id();
     129             : 
     130             :     // set external state for an instance
     131             :     void set_external(bool external);
     132             : 
     133             :     // tell if instance is an external compass
     134             :     bool is_external();
     135             : 
     136             :     // set rotation of an instance
     137             :     void set_rotation(enum Rotation rotation);
     138             : 
     139             :     // get board orientation (for SITL)
     140             :     enum Rotation get_board_orientation(void) const;
     141             :     
     142             :     // access to frontend
     143             :     Compass &_compass;
     144             : 
     145             :     // semaphore for access to shared frontend data
     146             :     HAL_Semaphore _sem;
     147             : 
     148             :     // Check that the compass field is valid by using a mean filter on the vector length
     149             :     bool field_ok(const Vector3f &field);
     150             :     
     151             :     uint32_t get_error_count() const { return _error_count; }
     152             : private:
     153             :     void apply_corrections(Vector3f &mag, uint8_t i);
     154             :     
     155             :     // mean field length for range filter
     156             :     float _mean_field_length;
     157             :     // number of dropped samples. Not used for now, but can be usable to choose more reliable sensor
     158             :     uint32_t _error_count;
     159             : };