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 : Simulate MicroStrain CX5 GNSS-INS devices
17 :
18 : Usage:
19 : PARAMS:
20 : param set AHRS_EKF_TYPE 11
21 : param set EAHRS_TYPE 2
22 : param set SERIAL3_PROTOCOL 36
23 : param set SERIAL3_BAUD 115
24 : sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain5" --console --map -DG
25 : */
26 : #include "SIM_MicroStrain.h"
27 : #include <stdio.h>
28 : #include <sys/time.h>
29 : #include <unistd.h>
30 : #include <fcntl.h>
31 : #include <AP_HAL/utility/sparse-endian.h>
32 : #include <GCS_MAVLink/GCS.h>
33 :
34 : using namespace SITL;
35 :
36 41 : MicroStrain::MicroStrain() :SerialDevice::SerialDevice()
37 : {
38 41 : }
39 :
40 :
41 36618 : void MicroStrain::simulation_timeval(struct timeval *tv)
42 : {
43 36618 : uint64_t now = AP_HAL::micros64();
44 : static uint64_t first_usec;
45 : static struct timeval first_tv;
46 36618 : if (first_usec == 0) {
47 41 : first_usec = now;
48 41 : first_tv.tv_sec = AP::sitl()->start_time_UTC;
49 : }
50 36618 : *tv = first_tv;
51 36618 : tv->tv_sec += now / 1000000ULL;
52 36618 : uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
53 36618 : tv->tv_sec += new_usec / 1000000ULL;
54 36618 : tv->tv_usec = new_usec % 1000000ULL;
55 36618 : }
56 :
57 36618 : void MicroStrain::generate_checksum(MicroStrain_Packet& packet)
58 : {
59 36618 : uint8_t checksumByte1 = 0;
60 36618 : uint8_t checksumByte2 = 0;
61 :
62 183090 : for (int i = 0; i < 4; i++) {
63 146472 : checksumByte1 += packet.header[i];
64 146472 : checksumByte2 += checksumByte1;
65 : }
66 :
67 2777322 : for (int i = 0; i < packet.header[3]; i++) {
68 2740704 : checksumByte1 += packet.payload[i];
69 2740704 : checksumByte2 += checksumByte1;
70 : }
71 :
72 36618 : packet.checksum[0] = checksumByte1;
73 36618 : packet.checksum[1] = checksumByte2;
74 36618 : }
75 :
76 36618 : void MicroStrain::send_packet(MicroStrain_Packet packet)
77 : {
78 36618 : generate_checksum(packet);
79 :
80 36618 : write_to_autopilot((char *)&packet.header, sizeof(packet.header));
81 36618 : write_to_autopilot((char *)&packet.payload, packet.payload_size);
82 36618 : write_to_autopilot((char *)&packet.checksum, sizeof(packet.checksum));
83 36618 : }
84 :
85 :
86 17258 : void MicroStrain::send_imu_packet(void)
87 : {
88 17258 : const auto &fdm = _sitl->state;
89 17258 : MicroStrain_Packet packet;
90 :
91 : struct timeval tv;
92 17258 : simulation_timeval(&tv);
93 :
94 17258 : if (start_us == 0) {
95 41 : start_us = tv.tv_usec * 1000;
96 : }
97 :
98 17258 : packet.header[0] = 0x75; // Sync One
99 17258 : packet.header[1] = 0x65; // Sync Two
100 17258 : packet.header[2] = 0x80; // INS Descriptor
101 :
102 : // Add ambient pressure field
103 17258 : packet.payload[packet.payload_size++] = 0x06; // Ambient Pressure Field Size
104 17258 : packet.payload[packet.payload_size++] = 0x17; // Descriptor
105 :
106 17258 : float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
107 17258 : put_float(packet, pressure_Pa*0.001 + rand_float() * 0.1);
108 :
109 : // Add scaled magnetometer field
110 17258 : packet.payload[packet.payload_size++] = 0x0E; // Scaled Magnetometer Field Size
111 17258 : packet.payload[packet.payload_size++] = 0x06; // Descriptor
112 17258 : put_float(packet, fdm.bodyMagField.x*0.001);
113 17258 : put_float(packet, fdm.bodyMagField.y*0.001);
114 17258 : put_float(packet, fdm.bodyMagField.z*0.001);
115 :
116 : // Add scaled accelerometer field
117 17258 : packet.payload[packet.payload_size++] = 0x0E; // Scaled Accelerometer Field Size
118 17258 : packet.payload[packet.payload_size++] = 0x04; // Descriptor
119 17258 : put_float(packet, fdm.xAccel / GRAVITY_MSS);
120 17258 : put_float(packet, fdm.yAccel / GRAVITY_MSS);
121 17258 : put_float(packet, fdm.zAccel / GRAVITY_MSS);
122 :
123 : // Add scaled gyro field
124 17258 : const float gyro_noise = 0.05;
125 17258 : packet.payload[packet.payload_size++] = 0x0E; // Scaled Gyro Field Size
126 17258 : packet.payload[packet.payload_size++] = 0x05; // Descriptor
127 17258 : put_float(packet, radians(fdm.rollRate + rand_float() * gyro_noise));
128 17258 : put_float(packet, radians(fdm.pitchRate + rand_float() * gyro_noise));
129 17258 : put_float(packet, radians(fdm.yawRate + rand_float() * gyro_noise));
130 :
131 : // Add CF Quaternion field
132 17258 : packet.payload[packet.payload_size++] = 0x12; // CF Quaternion Field Size
133 17258 : packet.payload[packet.payload_size++] = 0x0A; // Descriptor
134 17258 : put_float(packet, fdm.quaternion.q1);
135 17258 : put_float(packet, fdm.quaternion.q2);
136 17258 : put_float(packet, fdm.quaternion.q3);
137 17258 : put_float(packet, fdm.quaternion.q4);
138 :
139 17258 : packet.header[3] = packet.payload_size;
140 :
141 17258 : send_packet(packet);
142 17258 : }
143 :
144 656 : void MicroStrain5::send_gnss_packet(void)
145 : {
146 656 : const auto &fdm = _sitl->state;
147 656 : MicroStrain_Packet packet;
148 :
149 : struct timeval tv;
150 656 : simulation_timeval(&tv);
151 :
152 656 : packet.header[0] = 0x75; // Sync One
153 656 : packet.header[1] = 0x65; // Sync Two
154 656 : packet.header[2] = 0x81; // GNSS Descriptor
155 :
156 : // Add GPS Timestamp
157 656 : packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size
158 656 : packet.payload[packet.payload_size++] = 0xD3; // Descriptor
159 656 : put_double(packet, (double) tv.tv_sec);
160 656 : put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
161 656 : put_int(packet, 0);
162 :
163 : // Add GNSS Fix Information
164 656 : packet.payload[packet.payload_size++] = 0x08; // GNSS Fix Field Size
165 656 : packet.payload[packet.payload_size++] = 0x0B; // Descriptor
166 656 : packet.payload[packet.payload_size++] = 0x00; // Fix type
167 656 : packet.payload[packet.payload_size++] = 19; // Sat count
168 656 : put_int(packet, 0); // Fix flags
169 656 : put_int(packet, 0); // Valid flags
170 :
171 : // Add GNSS LLH position
172 656 : packet.payload[packet.payload_size++] = 0x2C; // GNSS LLH Field Size
173 656 : packet.payload[packet.payload_size++] = 0x03; // Descriptor
174 656 : put_double(packet, fdm.latitude);
175 656 : put_double(packet, fdm.longitude);
176 656 : put_double(packet, 0); // Height above ellipsoid - unused
177 656 : put_double(packet, fdm.altitude);
178 656 : put_float(packet, 0.5f); // Horizontal accuracy
179 656 : put_float(packet, 0.5f); // Vertical accuracy
180 656 : put_int(packet, 31); // Valid flags
181 :
182 : // Add DOP Data
183 656 : packet.payload[packet.payload_size++] = 0x20; // DOP Field Size
184 656 : packet.payload[packet.payload_size++] = 0x07; // Descriptor
185 656 : put_float(packet, 0); // GDOP
186 656 : put_float(packet, 0); // PDOP
187 656 : put_float(packet, 0); // HDOP
188 656 : put_float(packet, 0); // VDOP
189 656 : put_float(packet, 0); // TDOP
190 656 : put_float(packet, 0); // NDOP
191 656 : put_float(packet, 0); // EDOP
192 656 : put_int(packet, 127);
193 :
194 : // Add GNSS NED velocity
195 656 : packet.payload[packet.payload_size++] = 0x24; // GNSS NED Velocity Field Size
196 656 : packet.payload[packet.payload_size++] = 0x05; // Descriptor
197 656 : put_float(packet, fdm.speedN);
198 656 : put_float(packet, fdm.speedE);
199 656 : put_float(packet, fdm.speedD);
200 656 : put_float(packet, 0); //speed - unused
201 656 : put_float(packet, 0); //ground speed - unused
202 656 : put_float(packet, 0); //heading - unused
203 656 : put_float(packet, 0.25f); //speed accuracy
204 656 : put_float(packet, 0); //heading accuracy - unused
205 656 : put_int(packet, 31); //valid flags
206 :
207 656 : packet.header[3] = packet.payload_size;
208 :
209 :
210 656 : send_packet(packet);
211 656 : }
212 :
213 :
214 :
215 8209 : void MicroStrain5::send_filter_packet(void)
216 : {
217 8209 : const auto &fdm = _sitl->state;
218 8209 : MicroStrain_Packet packet;
219 :
220 : struct timeval tv;
221 8209 : simulation_timeval(&tv);
222 :
223 8209 : packet.header[0] = 0x75; // Sync One
224 8209 : packet.header[1] = 0x65; // Sync Two
225 8209 : packet.header[2] = 0x82; // Filter Descriptor
226 :
227 : // Add GPS Timestamp Shared Data
228 8209 : packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size
229 8209 : packet.payload[packet.payload_size++] = 0xD3; // Descriptor
230 8209 : put_double(packet, (double) tv.tv_usec / 1e6);
231 8209 : put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
232 8209 : put_int(packet, 0x0001);
233 :
234 : // Add GNSS Filter velocity
235 8209 : packet.payload[packet.payload_size++] = 0x10; // GNSS Velocity Field Size
236 8209 : packet.payload[packet.payload_size++] = 0x02; // Descriptor
237 8209 : put_float(packet, fdm.speedN);
238 8209 : put_float(packet, fdm.speedE);
239 8209 : put_float(packet, fdm.speedD);
240 8209 : put_int(packet, 0x0001);
241 :
242 : // Add Filter LLH position
243 8209 : packet.payload[packet.payload_size++] = 0x1C; // Filter LLH Field Size
244 8209 : packet.payload[packet.payload_size++] = 0x01; // Descriptor
245 8209 : put_double(packet, fdm.latitude);
246 8209 : put_double(packet, fdm.longitude);
247 8209 : put_double(packet, 0); // Height above ellipsoid - unused
248 8209 : put_int(packet, 0x0001); // Valid flags
249 :
250 : // Add Filter State
251 8209 : packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size
252 8209 : packet.payload[packet.payload_size++] = 0x10; // Descriptor
253 8209 : put_int(packet, 0x02); // Filter state (Running, Solution Valid)
254 8209 : put_int(packet, 0x03); // Dynamics mode (Airborne)
255 8209 : put_int(packet, 0); // Filter flags (None, no warnings)
256 :
257 8209 : packet.header[3] = packet.payload_size;
258 :
259 :
260 8209 : send_packet(packet);
261 8209 : }
262 :
263 : /*
264 : send MicroStrain data
265 : */
266 829836 : void MicroStrain::update(void)
267 : {
268 829836 : if (!init_sitl_pointer()) {
269 0 : return;
270 : }
271 :
272 829836 : uint32_t ms_between_imu_packets = 40;
273 829836 : uint32_t ms_between_gnss_packets = 500;
274 829836 : uint32_t ms_between_filter_packets = 40;
275 :
276 829836 : uint32_t now = AP_HAL::millis();
277 829836 : if (now - last_imu_pkt_ms >= ms_between_imu_packets) {
278 17258 : last_imu_pkt_ms = now;
279 17258 : send_imu_packet();
280 : }
281 :
282 829836 : if (now - last_gnss_pkt_ms >= ms_between_gnss_packets) {
283 1379 : last_gnss_pkt_ms = now;
284 1379 : send_gnss_packet();
285 : }
286 :
287 829836 : if (now - last_filter_pkt_ms >= ms_between_filter_packets) {
288 17258 : last_filter_pkt_ms = now;
289 17258 : send_filter_packet();
290 : }
291 : }
292 :
293 365316 : void MicroStrain::put_float(MicroStrain_Packet &packet, float f)
294 : {
295 365316 : uint32_t fbits = 0;
296 365316 : memcpy(&fbits, &f, sizeof(fbits));
297 365316 : put_be32_ptr(&packet.payload[packet.payload_size], fbits);
298 365316 : packet.payload_size += sizeof(float);
299 365316 : }
300 :
301 79542 : void MicroStrain::put_double(MicroStrain_Packet &packet, double d)
302 : {
303 79542 : uint64_t dbits = 0;
304 79542 : memcpy(&dbits, &d, sizeof(dbits));
305 79542 : put_be64_ptr(&packet.payload[packet.payload_size], dbits);
306 79542 : packet.payload_size += sizeof(double);
307 79542 : }
308 :
309 144569 : void MicroStrain::put_int(MicroStrain_Packet &packet, uint16_t t)
310 : {
311 144569 : put_be16_ptr(&packet.payload[packet.payload_size], t);
312 144569 : packet.payload_size += sizeof(uint16_t);
313 144569 : }
314 :
|
