Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_COMPASS_QMC5883L_ENABLED
       4             : 
       5             : #include "SIM_QMC5883L.h"
       6             : 
       7             : // FIXME: read the datasheet :-)
       8             : 
       9             : #include <SITL/SITL.h>
      10             : #include <SITL/SIM_Aircraft.h>
      11             : 
      12             : #include <stdio.h>
      13             : 
      14             : #define QMC5883L_REG_CONF1 0x09
      15             : #define QMC5883L_REG_CONF2 0x0A
      16             : 
      17             : #define QMC5883L_MODE_STANDBY 0x00
      18             : #define QMC5883L_MODE_CONTINUOUS 0x01
      19             : 
      20             : #define QMC5883L_ODR_100HZ (0x02 << 2)
      21             : 
      22             : #define QMC5883L_OSR_512 (0x00 << 6)
      23             : 
      24             : #define QMC5883L_RNG_8G (0x01 << 4)
      25             : 
      26        2145 : SITL::QMC5883L::QMC5883L()
      27             : {
      28        2145 :     writable_registers.set(0);
      29        2145 :     writable_registers.set(0x0b);
      30        2145 :     writable_registers.set(0x20);
      31        2145 :     writable_registers.set(0x21);
      32        2145 :     writable_registers.set(QMC5883L_REG_CONF1);
      33        2145 :     writable_registers.set(QMC5883L_REG_CONF2);
      34             : 
      35        2145 :     reset();
      36        2145 : }
      37             : 
      38        2145 : void SITL::QMC5883L::reset()
      39             : {
      40        2145 :     memset(registers.byte, 0, ARRAY_SIZE(registers.byte));
      41        2145 :     registers.byname.ZEROX_ZEROC = 0x01;
      42        2145 :     registers.byname.REG_STATUS = 0x0;
      43        2145 :     registers.byname.REG_ID = 0xFF;
      44        2145 : }
      45             : 
      46             : 
      47           0 : int SITL::QMC5883L::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      48             : {
      49           0 :     if (data->nmsgs == 2) {
      50           0 :         if (data->msgs[0].flags != 0) {
      51           0 :             AP_HAL::panic("Unexpected flags");
      52             :         }
      53           0 :         if (data->msgs[1].flags != I2C_M_RD) {
      54           0 :             AP_HAL::panic("Unexpected flags");
      55             :         }
      56           0 :         const uint8_t reg_addr = data->msgs[0].buf[0];
      57           0 :         for (uint8_t i=0; i<data->msgs[1].len; i++) {
      58           0 :             const uint8_t register_value = registers.byte[reg_addr+i];
      59           0 :             data->msgs[1].buf[i] = register_value;
      60             : 
      61             :             // FIXME: is this really how the status data-ready bit works?
      62           0 :             if (reg_addr == 0x05) {  // that's the last data register...
      63           0 :                 registers.byname.REG_STATUS &= ~0x04;
      64             :             }
      65             :         }
      66           0 :         return 0;
      67             :     }
      68             : 
      69           0 :     if (data->nmsgs == 1) {
      70           0 :         if (data->msgs[0].flags != 0) {
      71           0 :             AP_HAL::panic("Unexpected flags");
      72             :         }
      73           0 :         const uint8_t reg_addr = data->msgs[0].buf[0];
      74           0 :         if (!writable_registers.get(reg_addr)) {
      75           0 :             AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
      76             :         }
      77           0 :         const uint8_t register_value = data->msgs[0].buf[1];
      78           0 :         ::fprintf(stderr, "Setting register (0x%x) to 0x%x\n", reg_addr, register_value);
      79           0 :         registers.byte[reg_addr] = register_value;
      80           0 :         return 0;
      81             :     }
      82             : 
      83           0 :     return -1;
      84             : };
      85             : 
      86   178759942 : void SITL::QMC5883L::update(const Aircraft &aircraft)
      87             : {
      88             : 
      89             :     // FIXME: swipe stuff from AP_Compass_SITL here.
      90             : 
      91             :     // FIXME: somehow decide to use the simulated compass offsets etc
      92             :     // from SITL
      93             : 
      94   178759942 :     Vector3f f = AP::sitl()->state.bodyMagField;
      95             : 
      96             :     // Vector3<int16_t> field
      97             :     // int16_t str_x = 123;
      98             :     // int16_t str_y = -56;
      99             :     // int16_t str_z = 1;
     100             : 
     101   178759942 :     f.rotate_inverse(ROTATION_ROLL_180_YAW_270);
     102             : 
     103   178759942 :     f.x = -f.x;
     104   178759942 :     f.z = -f.z;
     105             : 
     106   178759942 :     f.x *= 3;
     107   178759942 :     f.y *= 3;
     108   178759942 :     f.z *= 3;
     109             : 
     110             :     Vector3<int16_t> k {
     111   178759942 :         k.x = htole16((int16_t)f.x),
     112   357519884 :         k.y = htole16((int16_t)f.y),
     113   357519884 :         k.z = htole16((int16_t)f.z)
     114   178759942 :     };
     115             : 
     116   178759942 :     if (registers.byname.REG_CONF1 & QMC5883L_MODE_CONTINUOUS) {
     117             :         // FIXME: clock according to configuration here
     118           0 :         registers.byname.REG_STATUS |= 0x04;
     119           0 :         registers.byname.DATA_OUTPUT_X_low = k.x & 0xFF;
     120           0 :         registers.byname.DATA_OUTPUT_X_high = k.x >> 8;
     121           0 :         registers.byname.DATA_OUTPUT_Y_low = k.y & 0xFF;
     122           0 :         registers.byname.DATA_OUTPUT_Y_high = k.y >> 8;
     123           0 :         registers.byname.DATA_OUTPUT_Z_low = k.z & 0xFF;
     124           0 :         registers.byname.DATA_OUTPUT_Z_high = k.z >> 8;
     125             :     }
     126   178759942 : }
     127             : 
     128             : #endif  // AP_SIM_COMPASS_QMC5883L_ENABLED