Line data Source code
1 : #include "SIM_Invensense_v3.h"
2 :
3 : #include <stdio.h>
4 :
5 178759942 : void SITL::InvensenseV3::update(const class Aircraft &aircraft)
6 : {
7 : ASSERT_STORAGE_SIZE(FIFOData, 16);
8 :
9 178759942 : const SIM *sitl = AP::sitl();
10 178759942 : const int16_t xAccel = sitl->state.xAccel / accel_scale();
11 178759942 : const int16_t yAccel = sitl->state.yAccel / accel_scale();
12 178759942 : const int16_t zAccel = sitl->state.zAccel / accel_scale();
13 :
14 178759942 : const int16_t p = radians(sitl->state.rollRate) / gyro_scale();
15 178759942 : const int16_t q = radians(sitl->state.pitchRate) / gyro_scale();
16 178759942 : const int16_t r = radians(sitl->state.yawRate) / gyro_scale();
17 :
18 178759942 : struct FIFOData new_data {
19 : 0x68,
20 : { xAccel, yAccel, zAccel },
21 : { p, q, r },
22 : 21, // temperature
23 178759942 : AP_HAL::millis16() // timestamp
24 178759942 : };
25 :
26 178767978 : for (uint8_t i=0; i<2; i++) {
27 178763960 : if (!write_to_fifo(InvensenseV3DevReg::FIFO_DATA, (uint8_t*)&new_data, sizeof(new_data))) {
28 178755924 : return;
29 : }
30 : }
31 4018 : update_sample_count();
32 : }
33 :
34 : // assert_register_values ensures register states when we go to do
35 : // various operations (e.g. reading from FIFO)
36 0 : void SITL::InvensenseV3::assert_register_values()
37 : {
38 : static const struct expected_register_values {
39 : uint8_t reg;
40 : uint8_t value;
41 : } expected[] {
42 : { InvensenseV3DevReg::FIFO_CONFIG, 0x80 },
43 : { InvensenseV3DevReg::FIFO_CONFIG1, 0x07 },
44 : { InvensenseV3DevReg::INTF_CONFIG0, 0xC0 },
45 : { InvensenseV3DevReg::SIGNAL_PATH_RESET, 2 },
46 : { InvensenseV3DevReg::PWR_MGMT0, 0x0f },
47 : { InvensenseV3DevReg::GYRO_CONFIG0, 0x05 },
48 : { InvensenseV3DevReg::ACCEL_CONFIG0, 0x05 },
49 : };
50 0 : for (const auto &stuff : expected) {
51 0 : assert_register_value(stuff.reg, stuff.value);
52 : }
53 0 : }
54 :
55 0 : int SITL::InvensenseV3::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
56 : {
57 0 : const uint8_t addr = data->msgs[0].buf[0];
58 :
59 : // see if it is a fifo...
60 0 : if (fifoname[addr] != nullptr) {
61 0 : return rdwr_fifo(data);
62 : }
63 : // see if it is a block...
64 0 : if (blockname[addr] != nullptr) {
65 0 : return rdwr_block(data);
66 : }
67 :
68 0 : return I2CRegisters_8Bit::rdwr(data);
69 : }
70 :
71 0 : int SITL::InvensenseV3::rdwr_fifo(I2C::i2c_rdwr_ioctl_data *&data)
72 : {
73 0 : const uint8_t addr = data->msgs[0].buf[0];
74 :
75 0 : assert_register_values();
76 :
77 : // check for block/FIFO read/write bits and pieces
78 0 : if (data->nmsgs == 2) {
79 0 : if (data->msgs[0].flags != 0) {
80 0 : AP_HAL::panic("Unexpected flags");
81 : }
82 0 : if (data->msgs[1].flags != I2C_M_RD) {
83 0 : AP_HAL::panic("Unexpected flags");
84 : }
85 :
86 0 : const uint8_t len = data->msgs[1].len;
87 0 : if (len > value_lengths[addr]) {
88 0 : if (value_lengths[addr] != 0) {
89 : // we expect reads and writes into the fifo to be the same size
90 0 : AP_HAL::panic("Read of unexpected size");
91 : }
92 0 : return -1;
93 : }
94 0 : memcpy(data->msgs[1].buf, values[addr], len);
95 0 : memmove(values[addr], values[addr]+len, value_lengths[addr]-len);
96 0 : value_lengths[addr] -= len;
97 0 : if (addr == InvensenseV3DevReg::FIFO_DATA) { // bit of a hack... callback?
98 0 : update_sample_count();
99 : }
100 0 : return 0;
101 : }
102 0 : return -1;
103 : }
104 :
105 2094 : void SITL::InvensenseV3::add_fifo(const char *name, uint8_t reg, I2CRegisters::RegMode mode)
106 : {
107 : // ::fprintf(stderr, "Adding fifo %u (0x%02x) (%s)\n", reg, reg, name);
108 2094 : fifoname[reg] = name;
109 2094 : if (mode == I2CRegisters::RegMode::RDONLY ||
110 : mode == I2CRegisters::RegMode::RDWR) {
111 2094 : readable_fifos.set((uint8_t)reg);
112 : }
113 2094 : if (mode == I2CRegisters::RegMode::WRONLY ||
114 : mode == I2CRegisters::RegMode::RDWR) {
115 0 : writable_fifos.set((uint8_t)reg);
116 : }
117 :
118 2094 : values[reg] = (char*)malloc(fifo_len); // allocate the fifo...
119 2094 : if (values[reg] == nullptr) {
120 0 : AP_HAL::panic("Failed to allocate FIFO...");
121 : }
122 2094 : }
123 :
124 12054 : void SITL::InvensenseV3::update_sample_count()
125 : {
126 12054 : if (value_lengths[InvensenseV3DevReg::FIFO_DATA] % sizeof(FIFOData)) {
127 0 : AP_HAL::panic("fifo data not multiple of sample size");
128 : }
129 12054 : uint16_t samplecount = value_lengths[InvensenseV3DevReg::FIFO_DATA]/sizeof(FIFOData);
130 12054 : set_block(InvensenseV3DevReg::FIFO_COUNTH, (uint8_t*)&samplecount, 2);
131 12054 : }
132 :
133 178763960 : bool SITL::InvensenseV3::write_to_fifo(uint8_t fifo, uint8_t *value, uint8_t valuelen)
134 : {
135 178763960 : if (fifoname[fifo] == nullptr) {
136 0 : AP_HAL::panic("Setting un-named fifo %u", fifo);
137 : }
138 : // ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
139 178763960 : if (valuelen == 0) {
140 0 : AP_HAL::panic("Zero-length values not permitted by spec");
141 : }
142 178763960 : if (values[fifo] == nullptr) {
143 0 : AP_HAL::panic("Write to unallocated FIFO");
144 : }
145 178763960 : if (value_lengths[fifo] + valuelen > fifo_len) {
146 : // ::fprintf(stderr, "dropped\n"); // this happens a lot at startup
147 178755924 : return false; // just drop it
148 : }
149 8036 : memcpy(&(values[fifo][value_lengths[fifo]]), value, valuelen);
150 8036 : value_lengths[fifo] += valuelen;
151 8036 : if (fifo == InvensenseV3DevReg::FIFO_DATA) { // bit of a hack... callback?
152 8036 : update_sample_count();
153 : }
154 8036 : return true;
155 : }
156 :
157 :
158 :
159 2094 : void SITL::InvensenseV3::add_block(const char *name, uint8_t addr, uint8_t len, I2CRegisters::RegMode mode)
160 : {
161 : // ::fprintf(stderr, "Adding block %u (0x%02x) (%s)\n", addr, addr, name);
162 2094 : blockname[addr] = name;
163 2094 : block_values[addr] = (char*)malloc(len);
164 2094 : block_value_lengths[addr] = len;
165 2094 : if (block_values[addr] == nullptr) {
166 0 : AP_HAL::panic("Allocation failed for block (len=%u)", len);
167 : }
168 2094 : if (mode == I2CRegisters::RegMode::RDONLY ||
169 : mode == I2CRegisters::RegMode::RDWR) {
170 2094 : readable_blocks.set((uint8_t)addr);
171 : }
172 2094 : if (mode == I2CRegisters::RegMode::WRONLY ||
173 : mode == I2CRegisters::RegMode::RDWR) {
174 0 : writable_blocks.set((uint8_t)addr);
175 : }
176 2094 : }
177 :
178 12054 : void SITL::InvensenseV3::set_block(uint8_t addr, uint8_t *value, uint8_t valuelen)
179 : {
180 12054 : if (blockname[addr] == nullptr) {
181 0 : AP_HAL::panic("Setting un-named block %u", addr);
182 : }
183 12054 : if (valuelen != block_value_lengths[addr]) {
184 0 : AP_HAL::panic("Invalid block write got=%u want=%u", valuelen, block_value_lengths[addr]);
185 : }
186 12054 : memcpy(block_values[addr], value, valuelen);
187 12054 : }
188 :
189 0 : int SITL::InvensenseV3::rdwr_block(I2C::i2c_rdwr_ioctl_data *&data)
190 : {
191 0 : const uint8_t addr = data->msgs[0].buf[0];
192 :
193 : // it is a block.
194 0 : if (data->nmsgs == 2) {
195 : // data read request
196 0 : if (data->msgs[0].flags != 0) {
197 0 : AP_HAL::panic("Unexpected flags");
198 : }
199 0 : if (data->msgs[1].flags != I2C_M_RD) {
200 0 : AP_HAL::panic("Unexpected flags");
201 : }
202 :
203 0 : if (data->msgs[1].len != block_value_lengths[addr]) {
204 0 : AP_HAL::panic("Block read length not equal to block length (got=%u want=%u)", data->msgs[1].len, block_value_lengths[addr]);
205 : }
206 0 : memcpy(&data->msgs[1].buf[0], block_values[addr], data->msgs[1].len);
207 0 : return 0;
208 : }
209 :
210 0 : if (data->nmsgs == 1) {
211 : // data write request
212 0 : if (data->msgs[0].flags != 0) {
213 0 : AP_HAL::panic("Unexpected flags");
214 : }
215 0 : AP_HAL::panic("block writes not implemented");
216 : }
217 :
218 0 : return -1;
219 : }
|
