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 GQ7 serially connected AHRS Systems
15 : Usage in SITL with hardware for debugging:
16 : $ sim_vehicle.py -v Plane -A "--serial3=uart:/dev/3dm-gq7" --console --map -DG
17 : $ ./Tools/autotest/sim_vehicle.py -v Plane -A "--serial3=uart:/dev/3dm-gq7" -DG
18 : param set AHRS_EKF_TYPE 11
19 : param set EAHRS_TYPE 7
20 : param set GPS1_TYPE 21
21 : param set GPS2_TYPE 21
22 : param set SERIAL3_BAUD 115
23 : param set SERIAL3_PROTOCOL 36
24 : UDEV rules for repeatable USB connection:
25 : $ cat /etc/udev/rules.d/99-usb-serial.rules
26 : SUBSYSTEM=="tty", ATTRS{manufacturer}=="Lord Microstrain", SYMLINK+="3dm-gq7"
27 : Usage with simulated MicroStrain7:
28 : ./Tools/autotest/sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG
29 : */
30 :
31 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
32 :
33 : #include "AP_ExternalAHRS_config.h"
34 :
35 : #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
36 :
37 : #include "AP_ExternalAHRS_MicroStrain7.h"
38 : #include "AP_Compass/AP_Compass_config.h"
39 : #include <AP_Baro/AP_Baro.h>
40 : #include <AP_Compass/AP_Compass.h>
41 : #include <AP_GPS/AP_GPS.h>
42 : #include <AP_HAL/utility/sparse-endian.h>
43 : #include <AP_InertialSensor/AP_InertialSensor.h>
44 : #include <GCS_MAVLink/GCS.h>
45 : #include <AP_Logger/AP_Logger.h>
46 : #include <AP_BoardConfig/AP_BoardConfig.h>
47 : #include <AP_SerialManager/AP_SerialManager.h>
48 :
49 : static const char* LOG_FMT = "%s ExternalAHRS: %s";
50 :
51 : extern const AP_HAL::HAL &hal;
52 :
53 14 : AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_frontend,
54 14 : AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
55 : {
56 14 : auto &sm = AP::serialmanager();
57 14 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
58 :
59 14 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
60 14 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
61 :
62 14 : if (!uart) {
63 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_FMT, get_name(), "no UART");
64 0 : return;
65 : }
66 :
67 14 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain7::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
68 0 : AP_BoardConfig::allocation_error("MicroStrain7 ExternalAHRS failed to allocate ExternalAHRS update thread");
69 : }
70 :
71 : // don't offer IMU by default, at 100Hz it is too slow for many aircraft
72 14 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) |
73 : uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
74 : uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
75 :
76 14 : hal.scheduler->delay(5000);
77 14 : if (!initialised()) {
78 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_FMT, get_name(), "failed to initialise.");
79 : }
80 : }
81 :
82 14 : void AP_ExternalAHRS_MicroStrain7::update_thread(void)
83 : {
84 14 : if (!port_open) {
85 14 : port_open = true;
86 14 : uart->begin(baudrate);
87 : }
88 :
89 : while (true) {
90 68933 : build_packet();
91 68933 : hal.scheduler->delay_microseconds(100);
92 68919 : check_initialise_state();
93 : }
94 : }
95 :
96 68919 : void AP_ExternalAHRS_MicroStrain7::check_initialise_state(void)
97 : {
98 68919 : const auto new_init_state = initialised();
99 : // Only send the message after fully booted up, otherwise it gets dropped.
100 68919 : if (!last_init_state && new_init_state && AP_HAL::millis() > 5000) {
101 14 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "initialised.");
102 14 : last_init_state = new_init_state;
103 : }
104 68919 : }
105 :
106 :
107 : // Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
108 85724 : void AP_ExternalAHRS_MicroStrain7::build_packet()
109 : {
110 85724 : if (uart == nullptr) {
111 0 : return;
112 : }
113 :
114 85724 : WITH_SEMAPHORE(sem);
115 85724 : uint32_t nbytes = MIN(uart->available(), 2048u);
116 1573412 : while (nbytes--> 0) {
117 : uint8_t b;
118 1487688 : if (!uart->read(b)) {
119 0 : break;
120 : }
121 : DescriptorSet descriptor;
122 1487688 : if (handle_byte(b, descriptor)) {
123 17240 : switch (descriptor) {
124 7982 : case DescriptorSet::IMUData:
125 7982 : post_imu();
126 7982 : break;
127 1276 : case DescriptorSet::GNSSData:
128 : case DescriptorSet::GNSSRecv1:
129 : case DescriptorSet::GNSSRecv2:
130 1276 : break;
131 7982 : case DescriptorSet::FilterData:
132 7982 : post_filter();
133 7982 : break;
134 0 : case DescriptorSet::BaseCommand:
135 : case DescriptorSet::DMCommand:
136 : case DescriptorSet::SystemCommand:
137 0 : break;
138 : }
139 : }
140 : }
141 85724 : }
142 :
143 :
144 :
145 : // Posts data from an imu packet to `state` and `handle_external` methods
146 7982 : void AP_ExternalAHRS_MicroStrain7::post_imu() const
147 : {
148 : {
149 7982 : WITH_SEMAPHORE(state.sem);
150 7982 : state.accel = imu_data.accel;
151 7982 : state.gyro = imu_data.gyro;
152 7982 : }
153 :
154 : {
155 : // *INDENT-OFF*
156 7982 : AP_ExternalAHRS::ins_data_message_t ins {
157 : accel: imu_data.accel,
158 : gyro: imu_data.gyro,
159 : temperature: -300
160 7982 : };
161 : // *INDENT-ON*
162 7982 : AP::ins().handle_external(ins);
163 : }
164 :
165 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
166 : {
167 : // *INDENT-OFF*
168 : AP_ExternalAHRS::mag_data_message_t mag {
169 : field: imu_data.mag
170 7982 : };
171 : // *INDENT-ON*
172 7982 : AP::compass().handle_external(mag);
173 : }
174 : #endif
175 :
176 : #if AP_BARO_EXTERNALAHRS_ENABLED
177 : {
178 : // *INDENT-OFF*
179 7982 : const AP_ExternalAHRS::baro_data_message_t baro {
180 : instance: 0,
181 7982 : pressure_pa: imu_data.pressure,
182 : // setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by MicroStrain
183 : temperature: 25,
184 7982 : };
185 : // *INDENT-ON*
186 7982 : AP::baro().handle_external(baro);
187 : }
188 : #endif
189 7982 : }
190 :
191 7982 : void AP_ExternalAHRS_MicroStrain7::post_filter() const
192 : {
193 : {
194 7982 : WITH_SEMAPHORE(state.sem);
195 7982 : state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
196 7982 : state.have_velocity = true;
197 :
198 : // TODO the filter does not supply MSL altitude.
199 : // The GNSS system has both MSL and WGS-84 ellipsoid height.
200 : // Use GNSS 0 even though it may be bad.
201 7982 : state.location = Location{filter_data.lat, filter_data.lon, gnss_data[0].msl_altitude, Location::AltFrame::ABSOLUTE};
202 7982 : state.have_location = true;
203 7982 : state.last_location_update_us = AP_HAL::micros();
204 :
205 7982 : state.quat = filter_data.attitude_quat;
206 7982 : state.have_quaternion = true;
207 7982 : }
208 :
209 23946 : for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) {
210 : // *INDENT-OFF*
211 : AP_ExternalAHRS::gps_data_message_t gps {
212 15964 : gps_week: filter_data.week,
213 15964 : ms_tow: filter_data.tow_ms,
214 15964 : fix_type: AP_GPS_FixType(gnss_data[instance].fix_type),
215 15964 : satellites_in_view: gnss_data[instance].satellites,
216 :
217 15964 : horizontal_pos_accuracy: gnss_data[instance].horizontal_position_accuracy,
218 15964 : vertical_pos_accuracy: gnss_data[instance].vertical_position_accuracy,
219 15964 : horizontal_vel_accuracy: gnss_data[instance].speed_accuracy,
220 :
221 15964 : hdop: gnss_data[instance].hdop,
222 15964 : vdop: gnss_data[instance].vdop,
223 :
224 15964 : longitude: gnss_data[instance].lon,
225 15964 : latitude: gnss_data[instance].lat,
226 15964 : msl_altitude: gnss_data[instance].msl_altitude,
227 :
228 15964 : ned_vel_north: gnss_data[instance].ned_velocity_north,
229 15964 : ned_vel_east: gnss_data[instance].ned_velocity_east,
230 15964 : ned_vel_down: gnss_data[instance].ned_velocity_down,
231 15964 : };
232 : // *INDENT-ON*
233 :
234 15964 : if (gps.fix_type >= AP_GPS_FixType::FIX_3D && !state.have_origin) {
235 14 : WITH_SEMAPHORE(state.sem);
236 28 : state.origin = Location{int32_t(gnss_data[instance].lat),
237 14 : int32_t(gnss_data[instance].lon),
238 14 : int32_t(gnss_data[instance].msl_altitude),
239 : Location::AltFrame::ABSOLUTE};
240 14 : state.have_origin = true;
241 14 : }
242 15964 : AP::gps().handle_external(gps, instance);
243 : }
244 7982 : }
245 :
246 41 : int8_t AP_ExternalAHRS_MicroStrain7::get_port(void) const
247 : {
248 41 : if (!uart) {
249 0 : return -1;
250 : }
251 41 : return port_num;
252 : };
253 :
254 : // Get model/type name
255 14 : const char* AP_ExternalAHRS_MicroStrain7::get_name() const
256 : {
257 14 : return "MicroStrain7";
258 : }
259 :
260 35608 : bool AP_ExternalAHRS_MicroStrain7::healthy(void) const
261 : {
262 35608 : return times_healthy() && filter_healthy();
263 : }
264 :
265 85802 : bool AP_ExternalAHRS_MicroStrain7::initialised(void) const
266 : {
267 85802 : const bool got_packets = last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
268 85802 : return got_packets;
269 : }
270 :
271 79 : bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
272 : {
273 79 : if (!initialised()) {
274 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "not initialised");
275 0 : return false;
276 : }
277 79 : if (!times_healthy()) {
278 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "data is stale");
279 0 : return false;
280 : }
281 79 : if (!filter_healthy()) {
282 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter is unhealthy");
283 0 : return false;
284 : }
285 79 : if (!healthy()) {
286 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "unhealthy");
287 0 : return false;
288 : }
289 : static_assert(NUM_GNSS_INSTANCES == 2, "This check only works if there are two GPS types.");
290 79 : if (gnss_data[0].fix_type < AP_GPS_FixType::FIX_3D && gnss_data[1].fix_type < AP_GPS_FixType::FIX_3D) {
291 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "missing 3D GPS fix on either GPS");
292 0 : return false;
293 : }
294 :
295 79 : return true;
296 : }
297 :
298 18738 : void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status) const
299 : {
300 18738 : memset(&status, 0, sizeof(status));
301 18738 : if (last_imu_pkt != 0 && last_gps_pkt != 0) {
302 18738 : status.flags.initalized = true;
303 : }
304 18738 : if (healthy() && last_imu_pkt != 0) {
305 18738 : status.flags.attitude = true;
306 18738 : status.flags.vert_vel = true;
307 18738 : status.flags.vert_pos = true;
308 :
309 18738 : const auto filter_state = static_cast<FilterState>(filter_status.state);
310 18738 : if (filter_state_healthy(filter_state)) {
311 18738 : status.flags.horiz_vel = true;
312 18738 : status.flags.horiz_pos_rel = true;
313 18738 : status.flags.horiz_pos_abs = true;
314 18738 : status.flags.pred_horiz_pos_rel = true;
315 18738 : status.flags.pred_horiz_pos_abs = true;
316 18738 : status.flags.using_gps = true;
317 : }
318 : }
319 18738 : }
320 :
321 : // get variances
322 18738 : bool AP_ExternalAHRS_MicroStrain7::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
323 : {
324 18738 : velVar = filter_data.ned_velocity_uncertainty.length() * vel_gate_scale;
325 18738 : posVar = filter_data.ned_position_uncertainty.xy().length() * pos_gate_scale;
326 18738 : hgtVar = filter_data.ned_position_uncertainty.z * hgt_gate_scale;
327 18738 : tasVar = 0;
328 18738 : return true;
329 : }
330 :
331 35687 : bool AP_ExternalAHRS_MicroStrain7::times_healthy() const
332 : {
333 35687 : uint32_t now = AP_HAL::millis();
334 :
335 : // Expect the following rates:
336 : // * Navigation Filter: 25Hz = 40mS
337 : // * GPS: 2Hz = 500mS
338 : // * IMU: 25Hz = 40mS
339 :
340 : // Allow for some slight variance of 10%
341 35687 : constexpr float RateFoS = 1.1;
342 :
343 35687 : constexpr uint32_t expected_filter_time_delta_ms = 40;
344 35687 : constexpr uint32_t expected_gps_time_delta_ms = 500;
345 35687 : constexpr uint32_t expected_imu_time_delta_ms = 40;
346 :
347 107061 : const bool times_healthy = (now - last_imu_pkt < expected_imu_time_delta_ms * RateFoS && \
348 71374 : now - last_gps_pkt < expected_gps_time_delta_ms * RateFoS && \
349 35687 : now - last_filter_pkt < expected_filter_time_delta_ms * RateFoS);
350 :
351 35687 : return times_healthy;
352 : }
353 :
354 35687 : bool AP_ExternalAHRS_MicroStrain7::filter_healthy() const
355 : {
356 35687 : const auto filter_state = static_cast<FilterState>(filter_status.state);
357 35687 : const bool filter_healthy = filter_state_healthy(filter_state);
358 35687 : return filter_healthy;
359 : }
360 :
361 54425 : bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state)
362 : {
363 54425 : switch (state) {
364 54425 : case FilterState::GQ7_FULL_NAV:
365 : case FilterState::GQ7_AHRS:
366 54425 : return true;
367 0 : default:
368 0 : return false;
369 : }
370 : }
371 :
372 : #endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
