Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   Driver by Voltafield, July 2026
      17             :  */
      18             : 
      19             : #include "AP_Compass_AF9838.h"
      20             : 
      21             : #if AP_COMPASS_AF9838_ENABLED
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : 
      25             : #include <AP_HAL/utility/sparse-endian.h>
      26             : 
      27             : // Resolution: 0.1 uT/LSB, converted to milligauss for AP_Compass.
      28             : static constexpr float AF9838_MILLIGAUSS_PER_LSB = 1.0f;
      29             : 
      30             : extern const AP_HAL::HAL &hal;
      31             : 
      32           0 : AP_Compass_AF9838::AP_Compass_AF9838(AP_HAL::OwnPtr<AP_HAL::Device> dev,
      33             :                                      bool force_external,
      34           0 :                                      enum Rotation rotation)
      35             :     : AP_Compass_Backend()
      36           0 :     , _dev(std::move(dev))
      37           0 :     , _force_external(force_external)
      38             : {
      39           0 :     set_rotation(rotation);
      40           0 : }
      41             : 
      42           0 : AP_Compass_Backend *AP_Compass_AF9838::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
      43             :         bool force_external,
      44             :         enum Rotation rotation)
      45             : {
      46           0 :     if (!dev) {
      47           0 :         return nullptr;
      48             :     }
      49             : 
      50           0 :     AP_Compass_AF9838 *sensor = NEW_NOTHROW AP_Compass_AF9838(std::move(dev), force_external, rotation);
      51           0 :     if (!sensor || !sensor->init()) {
      52           0 :         delete sensor;
      53           0 :         return nullptr;
      54             :     }
      55             : 
      56           0 :     return sensor;
      57             : }
      58             : 
      59           0 : bool AP_Compass_AF9838::init()
      60             : {
      61           0 :     WITH_SEMAPHORE(_dev->get_semaphore());
      62           0 :     _dev->set_retries(10);
      63             : 
      64           0 :     uint8_t val = 0;
      65             : 
      66           0 :     if (!_dev->read_registers(REG_PCODE, &val, 1) || val != PCODE_EXPECT) {
      67           0 :         return false;
      68             :     }
      69             : 
      70           0 :     if (!_dev->write_register(REG_SWR, SWR_TRIGGER)) {
      71           0 :         DEV_PRINTF("AF9838: reset failed\n");
      72           0 :         return false;
      73             :     }
      74           0 :     hal.scheduler->delay(5);
      75             : 
      76           0 :     if (!_dev->write_register(REG_STATE, STATE_SELF_TEST)) {
      77           0 :         DEV_PRINTF("AF9838: self-test start failed\n");
      78           0 :         return false;
      79             :     }
      80           0 :     hal.scheduler->delay(40);
      81             : 
      82           0 :     uint8_t status = 0;
      83             : 
      84           0 :     if (!_dev->read_registers(REG_STATUS, &status, 1)) {
      85           0 :         DEV_PRINTF("AF9838: self-test status read failed\n");
      86           0 :         return false;
      87             :     }
      88             : 
      89           0 :     if (status & STATUS_STERR) {
      90           0 :         DEV_PRINTF("AF9838: self-test failed\n");
      91             :     }
      92             : 
      93           0 :     _dev->set_device_type(DEVTYPE_AF9838);
      94             : 
      95           0 :     if (!register_compass(_dev->get_bus_id())) {
      96           0 :         DEV_PRINTF("AF9838: compass registration failed\n");
      97           0 :         return false;
      98             :     }
      99             : 
     100           0 :     if (_force_external) {
     101           0 :         set_external(true);
     102             :     }
     103             : 
     104           0 :     _dev->set_retries(3);
     105             : 
     106           0 :     _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AF9838::timer, void));
     107             : 
     108           0 :     return true;
     109           0 : }
     110             : 
     111           0 : void AP_Compass_AF9838::timer()
     112             : {
     113             : 
     114           0 :     if (!_single_pending) {
     115           0 :         if (!_dev->write_register(REG_STATE, STATE_SINGLE)) {
     116           0 :             return;
     117             :         }
     118           0 :         _single_pending = true;
     119           0 :         _single_start_us = AP_HAL::micros();
     120           0 :         return;
     121             :     }
     122             : 
     123           0 :     uint8_t status = 0;
     124           0 :     if (!_dev->read_registers(REG_STATUS, &status, 1)) {
     125           0 :         _single_pending = false;
     126           0 :         return;
     127             :     }
     128             : 
     129           0 :     if (status & STATUS_HOFL) {
     130             :     // HOFL is cleared when the next measurement starts.
     131           0 :     _single_pending = false;
     132           0 :     return;
     133             :     }
     134             : 
     135           0 :     if ((status & STATUS_ACQ) == 0) {
     136           0 :         const uint32_t now = AP_HAL::micros();
     137           0 :         if ((now - _single_start_us) > 20000U) {
     138           0 :             _single_pending = false;
     139             :         }
     140           0 :         return;
     141             :     }
     142             : 
     143             :     uint8_t buf[6];
     144           0 :     if (!_dev->read_registers(REG_DATA, buf, sizeof(buf))) {
     145           0 :         _single_pending = false;
     146           0 :         return;
     147             :     }
     148             : 
     149             :     // Raw samples are little-endian signed 16-bit values.
     150           0 :     const int16_t x = (int16_t)le16toh_ptr(&buf[0]);
     151           0 :     const int16_t y = (int16_t)le16toh_ptr(&buf[2]);
     152           0 :     const int16_t z = (int16_t)le16toh_ptr(&buf[4]);
     153             : 
     154             :     Vector3f field{
     155             :         float(x) * AF9838_MILLIGAUSS_PER_LSB,
     156             :         float(y) * AF9838_MILLIGAUSS_PER_LSB,
     157             :         float(z) * AF9838_MILLIGAUSS_PER_LSB
     158           0 :     };
     159             : 
     160           0 :     accumulate_sample(field);
     161             : 
     162           0 :     if (_dev->write_register(REG_STATE, STATE_SINGLE)) {
     163           0 :         _single_pending = true;
     164           0 :         _single_start_us = AP_HAL::micros();
     165             :     } else {
     166           0 :         _single_pending = false;
     167             :     }
     168             : }
     169             : 
     170             : #endif  // AP_COMPASS_AF9838_ENABLED