CoCalc -- AP_Compass

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp
Views: ¹⁷⁹⁸

1
#include "AP_Compass_LSM9DS1.h"
2

3
#if AP_COMPASS_LSM9DS1_ENABLED
4

5
#include <AP_Math/AP_Math.h>
6
#include <AP_HAL/AP_HAL.h>
7

8
#define LSM9DS1M_OFFSET_X_REG_L_M   0x05
9
#define LSM9DS1M_OFFSET_X_REG_H_M   0x06
10
#define LSM9DS1M_OFFSET_Y_REG_L_M   0x07
11
#define LSM9DS1M_OFFSET_Y_REG_H_M   0x08
12
#define LSM9DS1M_OFFSET_Z_REG_L_M   0x09
13
#define LSM9DS1M_OFFSET_Z_REG_H_M   0x0A
14

15
#define LSM9DS1M_WHO_AM_I           0x0F
16
    #define WHO_AM_I_MAG            0x3D
17

18
#define LSM9DS1M_CTRL_REG1_M        0x20
19
    #define LSM9DS1M_TEMP_COMP      (0x1 << 7)
20
    #define LSM9DS1M_XY_ULTRA_HIGH  (0x3 << 5)
21
    #define LSM9DS1M_80HZ           (0x7 << 2)
22
    #define LSM9DS1M_FAST_ODR       (0x1 << 1)
23

24
#define LSM9DS1M_CTRL_REG2_M        0x21
25
    #define LSM9DS1M_FS_16G         (0x3 << 5)
26

27
#define LSM9DS1M_CTRL_REG3_M        0x22
28
    #define LSM9DS1M_SPI_ENABLE      (0x01 << 2)
29
    #define LSM9DS1M_CONTINUOUS_MODE 0x0
30

31
#define LSM9DS1M_CTRL_REG4_M        0x23
32
    #define LSM9DS1M_Z_ULTRA_HIGH  (0x3 << 2)
33

34
#define LSM9DS1M_CTRL_REG5_M        0x24
35
    #define LSM9DS1M_BDU            (0x0 << 6)
36

37
#define LSM9DS1M_STATUS_REG_M       0x27
38

39
#define LSM9DS1M_OUT_X_L_M          0x28
40
#define LSM9DS1M_OUT_X_H_M          0x29
41
#define LSM9DS1M_OUT_Y_L_M          0x2A
42
#define LSM9DS1M_OUT_Y_H_M          0x2B
43
#define LSM9DS1M_OUT_Z_L_M          0x2C
44
#define LSM9DS1M_OUT_Z_H_M          0x2D
45
#define LSM9DS1M_INT_CFG_M          0x30
46
#define LSM9DS1M_INT_SRC_M          0x31
47
#define LSM9DS1M_INT_THS_L_M        0x32
48
#define LSM9DS1M_INT_THS_H_M        0x33
49

50
extern const AP_HAL::HAL &hal;
51

52
AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(AP_HAL::OwnPtr<AP_HAL::Device> dev,
53
                                       enum Rotation rotation)
54
    : _dev(std::move(dev))
55
    , _rotation(rotation)
56
{
57
}
58

59
AP_Compass_Backend *AP_Compass_LSM9DS1::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
60
                                              enum Rotation rotation)
61
{
62
    if (!dev) {
63
        return nullptr;
64
    }
65
    AP_Compass_LSM9DS1 *sensor = NEW_NOTHROW AP_Compass_LSM9DS1(std::move(dev), rotation);
66
    if (!sensor || !sensor->init()) {
67
        delete sensor;
68
        return nullptr;
69
    }
70

71
    return sensor;
72
}
73

74
bool AP_Compass_LSM9DS1::init()
75
{
76
    AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
77

78
    if (!bus_sem) {
79
        DEV_PRINTF("LSM9DS1: Unable to get bus semaphore\n");
80
        return false;
81
    }
82
    bus_sem->take_blocking();
83

84
    if (!_check_id()) {
85
        DEV_PRINTF("LSM9DS1: Could not check id\n");
86
        goto errout;
87
    }
88

89
    if (!_configure()) {
90
        DEV_PRINTF("LSM9DS1: Could not check id\n");
91
        goto errout;
92
    }
93

94
    if (!_set_scale()) {
95
        DEV_PRINTF("LSM9DS1: Could not set scale\n");
96
        goto errout;
97
    }
98

99
    //register compass instance
100
    _dev->set_device_type(DEVTYPE_LSM9DS1);
101
    if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
102
        goto errout;
103
    }
104
    set_dev_id(_compass_instance, _dev->get_bus_id());
105

106
    set_rotation(_compass_instance, _rotation);
107

108

109
    _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, void));
110

111
    bus_sem->give();
112

113
    return true;
114

115
errout:
116
    bus_sem->give();
117
    return false;
118
}
119

120
void AP_Compass_LSM9DS1::_dump_registers()
121
{
122
    DEV_PRINTF("LSMDS1 registers\n");
123
    for (uint8_t reg = LSM9DS1M_OFFSET_X_REG_L_M; reg <= LSM9DS1M_INT_THS_H_M; reg++) {
124
        DEV_PRINTF("%02x:%02x ", (unsigned)reg, (unsigned)_register_read(reg));
125
        if ((reg - (LSM9DS1M_OFFSET_X_REG_L_M-1)) % 16 == 0) {
126
            DEV_PRINTF("\n");
127
        }
128
    }
129
    DEV_PRINTF("\n");
130
}
131

132
void AP_Compass_LSM9DS1::_update(void)
133
{
134
    struct sample_regs regs;
135
    Vector3f raw_field;
136

137
    if (!_block_read(LSM9DS1M_STATUS_REG_M, (uint8_t *) &regs, sizeof(regs))) {
138
        return;
139
    }
140

141
    if (regs.status & 0x80) {
142
        return;
143
    }
144

145
    raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
146

147
    if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
148
        return;
149
    }
150

151
    raw_field *= _scaling;
152

153
    accumulate_sample(raw_field, _compass_instance);
154
}
155

156
void AP_Compass_LSM9DS1::read()
157
{
158
    drain_accumulated_samples(_compass_instance);
159
}
160

161
bool AP_Compass_LSM9DS1::_check_id(void)
162
{
163
    // initially run the bus at low speed
164
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);
165

166
    uint8_t value = _register_read(LSM9DS1M_WHO_AM_I);
167
    if (value != WHO_AM_I_MAG) {
168
        DEV_PRINTF("LSM9DS1: unexpected WHOAMI 0x%x\n", (unsigned)value);
169
        return false;
170
    }
171

172
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
173

174
    return true;
175
}
176

177
bool AP_Compass_LSM9DS1::_configure(void)
178
{
179
    _register_write(LSM9DS1M_CTRL_REG1_M, LSM9DS1M_TEMP_COMP | LSM9DS1M_XY_ULTRA_HIGH | LSM9DS1M_80HZ | LSM9DS1M_FAST_ODR);
180
    _register_write(LSM9DS1M_CTRL_REG2_M, LSM9DS1M_FS_16G);
181
    _register_write(LSM9DS1M_CTRL_REG3_M, LSM9DS1M_CONTINUOUS_MODE);
182
    _register_write(LSM9DS1M_CTRL_REG4_M, LSM9DS1M_Z_ULTRA_HIGH);
183
    _register_write(LSM9DS1M_CTRL_REG5_M, LSM9DS1M_BDU);
184

185
    return true;
186
}
187

188
bool AP_Compass_LSM9DS1::_set_scale(void)
189
{
190
    static const uint8_t FS_M_MASK = 0xc;
191
    _register_modify(LSM9DS1M_CTRL_REG2_M, FS_M_MASK, LSM9DS1M_FS_16G);
192
    _scaling = 0.58f;
193

194
    return true;
195
}
196

197
uint8_t AP_Compass_LSM9DS1::_register_read(uint8_t reg)
198
{
199
    uint8_t val = 0;
200

201
    /* set READ bit */
202
    reg |= 0x80;
203
    _dev->read_registers(reg, &val, 1);
204

205
    return val;
206
}
207

208
bool AP_Compass_LSM9DS1::_block_read(uint8_t reg, uint8_t *buf, uint32_t size)
209
{
210
    /* set !MS bit */
211
    reg |= 0xc0;
212
    return _dev->read_registers(reg, buf, size);
213
}
214

215
void AP_Compass_LSM9DS1::_register_write(uint8_t reg, uint8_t val)
216
{
217
    _dev->write_register(reg, val);
218
}
219

220
void AP_Compass_LSM9DS1::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits)
221
{
222
    uint8_t val;
223

224
    val = _register_read(reg);
225
    val &= ~clearbits;
226
    val |= setbits;
227
    _register_write(reg, val);
228
}
229

230
#endif
231

232

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.