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 : This program is distributed in the hope that it will be useful,
7 : but WITHOUT ANY WARRANTY; without even the implied warranty of
8 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 : GNU General Public License for more details.
10 : You should have received a copy of the GNU General Public License
11 : along with this program. If not, see <http://www.gnu.org/licenses/>.
12 : */
13 : /*
14 : support for MicroStrain MIP parsing
15 : */
16 :
17 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
18 :
19 : #include "AP_ExternalAHRS_config.h"
20 : #include <AP_GPS/AP_GPS.h>
21 :
22 : #if AP_MICROSTRAIN_ENABLED
23 :
24 : #include "MicroStrain_common.h"
25 : #include <AP_HAL/utility/sparse-endian.h>
26 :
27 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/sensor_data/sensor_data_links.htm
28 : enum class INSPacketField {
29 : ACCEL = 0x04,
30 : GYRO = 0x05,
31 : QUAT = 0x0A,
32 : MAG = 0x06,
33 : PRESSURE = 0x17,
34 : };
35 :
36 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/gnss_recv_1_links.htm
37 : enum class GNSSPacketField {
38 : LLH_POSITION = 0x03,
39 : NED_VELOCITY = 0x05,
40 : DOP_DATA = 0x07,
41 : FIX_INFO = 0x0B,
42 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
43 : GPS_TIMESTAMP = 0xD3,
44 : };
45 :
46 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm
47 : enum class GNSSFixType {
48 : FIX_3D = 0x00,
49 : FIX_2D = 0x01,
50 : TIME_ONLY = 0x02,
51 : NONE = 0x03,
52 : INVALID = 0x04,
53 : FIX_RTK_FLOAT = 0x05,
54 : FIX_RTK_FIXED = 0x06,
55 : };
56 :
57 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/filter_data_links.htm
58 : enum class FilterPacketField {
59 : FILTER_STATUS = 0x10,
60 : LLH_POSITION = 0x01,
61 : NED_VELOCITY = 0x02,
62 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm
63 : ATTITUDE_QUAT = 0x03,
64 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/0x82/data/0x08.htm
65 : LLH_POSITION_UNCERTAINTY = 0x08,
66 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/0x82/data/0x09.htm
67 : NED_VELOCITY_UNCERTAINTY = 0x09,
68 : // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
69 : GPS_TIMESTAMP = 0xD3,
70 : };
71 :
72 2674108 : bool AP_MicroStrain::handle_byte(const uint8_t b, DescriptorSet& descriptor)
73 : {
74 2674108 : switch (message_in.state) {
75 33141 : case ParseState::WaitingFor_SyncOne:
76 33141 : if (b == SYNC_ONE) {
77 33141 : message_in.packet.header[0] = b;
78 33141 : message_in.state = ParseState::WaitingFor_SyncTwo;
79 : }
80 33141 : break;
81 33141 : case ParseState::WaitingFor_SyncTwo:
82 33141 : if (b == SYNC_TWO) {
83 33141 : message_in.packet.header[1] = b;
84 33141 : message_in.state = ParseState::WaitingFor_Descriptor;
85 : } else {
86 0 : message_in.state = ParseState::WaitingFor_SyncOne;
87 : }
88 33141 : break;
89 33141 : case ParseState::WaitingFor_Descriptor:
90 33141 : message_in.packet.descriptor_set(b);
91 33141 : message_in.state = ParseState::WaitingFor_PayloadLength;
92 33141 : break;
93 33141 : case ParseState::WaitingFor_PayloadLength:
94 33141 : message_in.packet.payload_length(b);
95 33141 : message_in.state = ParseState::WaitingFor_Data;
96 33141 : message_in.index = 0;
97 33141 : break;
98 2475262 : case ParseState::WaitingFor_Data:
99 2475262 : message_in.packet.payload[message_in.index++] = b;
100 2475262 : if (message_in.index >= message_in.packet.payload_length()) {
101 33141 : message_in.state = ParseState::WaitingFor_Checksum;
102 33141 : message_in.index = 0;
103 : }
104 2475262 : break;
105 66282 : case ParseState::WaitingFor_Checksum:
106 66282 : message_in.packet.checksum[message_in.index++] = b;
107 66282 : if (message_in.index >= 2) {
108 33141 : message_in.state = ParseState::WaitingFor_SyncOne;
109 33141 : message_in.index = 0;
110 :
111 33141 : if (valid_packet(message_in.packet)) {
112 33141 : descriptor = handle_packet(message_in.packet);
113 33141 : return true;
114 : }
115 : }
116 33141 : break;
117 : }
118 2640967 : return false;
119 : }
120 :
121 33141 : bool AP_MicroStrain::valid_packet(const MicroStrain_Packet & packet)
122 : {
123 33141 : uint8_t checksum_one = 0;
124 33141 : uint8_t checksum_two = 0;
125 :
126 165705 : for (int i = 0; i < 4; i++) {
127 132564 : checksum_one += packet.header[i];
128 132564 : checksum_two += checksum_one;
129 : }
130 :
131 2508403 : for (int i = 0; i < packet.payload_length(); i++) {
132 2475262 : checksum_one += packet.payload[i];
133 2475262 : checksum_two += checksum_one;
134 : }
135 :
136 33141 : return packet.checksum[0] == checksum_one && packet.checksum[1] == checksum_two;
137 : }
138 :
139 33141 : AP_MicroStrain::DescriptorSet AP_MicroStrain::handle_packet(const MicroStrain_Packet& packet)
140 : {
141 33141 : const DescriptorSet descriptor = packet.descriptor_set();
142 33141 : switch (descriptor) {
143 15627 : case DescriptorSet::IMUData:
144 15627 : handle_imu(packet);
145 15627 : break;
146 15627 : case DescriptorSet::FilterData:
147 15627 : handle_filter(packet);
148 15627 : break;
149 0 : case DescriptorSet::BaseCommand:
150 : case DescriptorSet::DMCommand:
151 : case DescriptorSet::SystemCommand:
152 0 : break;
153 1887 : case DescriptorSet::GNSSData:
154 : case DescriptorSet::GNSSRecv1:
155 : case DescriptorSet::GNSSRecv2:
156 1887 : handle_gnss(packet);
157 1887 : break;
158 : }
159 33141 : return descriptor;
160 : }
161 :
162 :
163 15627 : void AP_MicroStrain::handle_imu(const MicroStrain_Packet& packet)
164 : {
165 15627 : last_imu_pkt = AP_HAL::millis();
166 :
167 : // Iterate through fields of varying lengths in INS packet
168 93762 : for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) {
169 78135 : switch ((INSPacketField) packet.payload[i+1]) {
170 : // Scaled Ambient Pressure
171 15627 : case INSPacketField::PRESSURE: {
172 15627 : imu_data.pressure = be32tofloat_ptr(packet.payload, i+2) * 100; // Convert millibar to pascals
173 15627 : break;
174 : }
175 : // Scaled Magnetometer Vector
176 15627 : case INSPacketField::MAG: {
177 15627 : imu_data.mag = populate_vector3f(packet.payload, i+2) * 1000; // Convert gauss to milligauss
178 15627 : break;
179 : }
180 : // Scaled Accelerometer Vector
181 15627 : case INSPacketField::ACCEL: {
182 15627 : imu_data.accel = populate_vector3f(packet.payload, i+2) * GRAVITY_MSS; // Convert g's to m/s^2
183 15627 : break;
184 : }
185 : // Scaled Gyro Vector
186 15627 : case INSPacketField::GYRO: {
187 15627 : imu_data.gyro = populate_vector3f(packet.payload, i+2);
188 15627 : break;
189 : }
190 : // Quaternion
191 15627 : case INSPacketField::QUAT: {
192 15627 : imu_data.quat = populate_quaternion(packet.payload, i+2);
193 15627 : break;
194 : }
195 : }
196 : }
197 15627 : }
198 :
199 :
200 1887 : void AP_MicroStrain::handle_gnss(const MicroStrain_Packet &packet)
201 : {
202 1887 : last_gps_pkt = AP_HAL::millis();
203 : uint8_t gnss_instance;
204 1887 : const DescriptorSet descriptor = DescriptorSet(packet.descriptor_set());
205 1887 : if (!get_gnss_instance(descriptor, gnss_instance)) {
206 0 : return;
207 : }
208 :
209 : // Iterate through fields of varying lengths in GNSS packet
210 11322 : for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) {
211 9435 : switch ((GNSSPacketField) packet.payload[i+1]) {
212 1887 : case GNSSPacketField::GPS_TIMESTAMP: {
213 1887 : gnss_data[gnss_instance].tow_ms = double_to_uint32(be64todouble_ptr(packet.payload, i+2) * 1000); // Convert seconds to ms
214 1887 : gnss_data[gnss_instance].week = be16toh_ptr(&packet.payload[i+10]);
215 1887 : break;
216 : }
217 1887 : case GNSSPacketField::FIX_INFO: {
218 1887 : switch ((GNSSFixType) packet.payload[i+2]) {
219 0 : case (GNSSFixType::FIX_RTK_FLOAT): {
220 0 : gnss_data[gnss_instance].fix_type = AP_GPS_FixType::RTK_FLOAT;
221 0 : break;
222 : }
223 0 : case (GNSSFixType::FIX_RTK_FIXED): {
224 0 : gnss_data[gnss_instance].fix_type = AP_GPS_FixType::RTK_FIXED;
225 0 : break;
226 : }
227 1887 : case (GNSSFixType::FIX_3D): {
228 1887 : gnss_data[gnss_instance].fix_type = AP_GPS_FixType::FIX_3D;
229 1887 : break;
230 : }
231 0 : case (GNSSFixType::FIX_2D): {
232 0 : gnss_data[gnss_instance].fix_type = AP_GPS_FixType::FIX_2D;
233 0 : break;
234 : }
235 0 : case (GNSSFixType::TIME_ONLY):
236 : case (GNSSFixType::NONE): {
237 0 : gnss_data[gnss_instance].fix_type = AP_GPS_FixType::NONE;
238 0 : break;
239 : }
240 0 : default:
241 : case (GNSSFixType::INVALID): {
242 0 : gnss_data[gnss_instance].fix_type = AP_GPS_FixType::NO_GPS;
243 0 : break;
244 : }
245 : }
246 :
247 1887 : gnss_data[gnss_instance].satellites = packet.payload[i+3];
248 1887 : break;
249 : }
250 1887 : case GNSSPacketField::LLH_POSITION: {
251 1887 : gnss_data[gnss_instance].lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
252 1887 : gnss_data[gnss_instance].lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
253 1887 : gnss_data[gnss_instance].msl_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
254 1887 : gnss_data[gnss_instance].horizontal_position_accuracy = be32tofloat_ptr(packet.payload, i+34);
255 1887 : gnss_data[gnss_instance].vertical_position_accuracy = be32tofloat_ptr(packet.payload, i+38);
256 1887 : break;
257 : }
258 1887 : case GNSSPacketField::DOP_DATA: {
259 1887 : gnss_data[gnss_instance].hdop = be32tofloat_ptr(packet.payload, i+10);
260 1887 : gnss_data[gnss_instance].vdop = be32tofloat_ptr(packet.payload, i+14);
261 1887 : break;
262 : }
263 1887 : case GNSSPacketField::NED_VELOCITY: {
264 1887 : gnss_data[gnss_instance].ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
265 1887 : gnss_data[gnss_instance].ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
266 1887 : gnss_data[gnss_instance].ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
267 1887 : gnss_data[gnss_instance].speed_accuracy = be32tofloat_ptr(packet.payload, i+26);
268 1887 : break;
269 : }
270 : }
271 : }
272 : }
273 :
274 15627 : void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet)
275 : {
276 15627 : last_filter_pkt = AP_HAL::millis();
277 :
278 : // Iterate through fields of varying lengths in filter packet
279 86117 : for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) {
280 70490 : switch ((FilterPacketField) packet.payload[i+1]) {
281 15627 : case FilterPacketField::GPS_TIMESTAMP: {
282 15627 : filter_data.tow_ms = be64todouble_ptr(packet.payload, i+2) * 1000; // Convert seconds to ms
283 15627 : filter_data.week = be16toh_ptr(&packet.payload[i+10]);
284 15627 : break;
285 : }
286 15627 : case FilterPacketField::LLH_POSITION: {
287 15627 : filter_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
288 15627 : filter_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
289 15627 : filter_data.hae_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
290 15627 : break;
291 : }
292 15627 : case FilterPacketField::NED_VELOCITY: {
293 15627 : filter_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
294 15627 : filter_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
295 15627 : filter_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
296 15627 : break;
297 : }
298 7982 : case FilterPacketField::ATTITUDE_QUAT: {
299 7982 : filter_data.attitude_quat = populate_quaternion(packet.payload, i+2);
300 7982 : filter_data.attitude_quat.normalize();
301 7982 : break;
302 : }
303 0 : case FilterPacketField::LLH_POSITION_UNCERTAINTY: {
304 0 : const float north_pos_acc = be32tofloat_ptr(packet.payload, i+2);
305 0 : const float east_pos_acc = be32tofloat_ptr(packet.payload, i+6);
306 0 : const float down_pos_acc = be32tofloat_ptr(packet.payload, i+10);
307 0 : filter_data.ned_position_uncertainty = Vector3f(
308 : north_pos_acc,
309 : east_pos_acc,
310 : down_pos_acc
311 : );
312 0 : break;
313 : }
314 0 : case FilterPacketField::NED_VELOCITY_UNCERTAINTY: {
315 0 : const float north_vel_uncertainty = be32tofloat_ptr(packet.payload, i+2);
316 0 : const float east_vel_uncertainty = be32tofloat_ptr(packet.payload, i+6);
317 0 : const float down_vel_uncertainty = be32tofloat_ptr(packet.payload, i+10);
318 0 : filter_data.ned_velocity_uncertainty = Vector3f(
319 : north_vel_uncertainty,
320 : east_vel_uncertainty,
321 : down_vel_uncertainty
322 : );
323 0 : break;
324 : }
325 15627 : case FilterPacketField::FILTER_STATUS: {
326 15627 : filter_status.state = be16toh_ptr(&packet.payload[i+2]);
327 15627 : filter_status.mode = be16toh_ptr(&packet.payload[i+4]);
328 15627 : filter_status.flags = be16toh_ptr(&packet.payload[i+6]);
329 15627 : break;
330 : }
331 : }
332 : }
333 15627 : }
334 :
335 46881 : Vector3f AP_MicroStrain::populate_vector3f(const uint8_t *data, uint8_t offset)
336 : {
337 93762 : return Vector3f {
338 : be32tofloat_ptr(data, offset),
339 46881 : be32tofloat_ptr(data, offset+4),
340 46881 : be32tofloat_ptr(data, offset+8)
341 46881 : };
342 : }
343 :
344 23609 : Quaternion AP_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offset)
345 : {
346 : // https://github.com/clemense/quaternion-conventions
347 : // AP follows W + Xi + Yj + Zk format.
348 : // Microstrain follows the same
349 47218 : return Quaternion {
350 : be32tofloat_ptr(data, offset),
351 23609 : be32tofloat_ptr(data, offset+4),
352 23609 : be32tofloat_ptr(data, offset+8),
353 23609 : be32tofloat_ptr(data, offset+12)
354 23609 : };
355 : }
356 :
357 1887 : bool AP_MicroStrain::get_gnss_instance(const DescriptorSet& descriptor, uint8_t& instance){
358 1887 : bool success = false;
359 :
360 1887 : switch(descriptor) {
361 1249 : case DescriptorSet::GNSSData:
362 : case DescriptorSet::GNSSRecv1:
363 1249 : instance = 0;
364 1249 : success = true;
365 1249 : break;
366 638 : case DescriptorSet::GNSSRecv2:
367 638 : instance = 1;
368 638 : success = true;
369 638 : break;
370 0 : default:
371 0 : break;
372 : }
373 1887 : return success;
374 : }
375 :
376 :
377 :
378 : #endif // AP_MICROSTRAIN_ENABLED
|
