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 : Support for SensAItion serial connected INS and IMU
17 : Implements SensAItion protocol with ArduPilot-specific adaptations
18 : */
19 :
20 : #include "AP_ExternalAHRS_SensAItion.h"
21 :
22 : #if AP_EXTERNAL_AHRS_SENSAITION_ENABLED
23 :
24 : #include <float.h>
25 : #include <math.h>
26 :
27 : #include <AP_SerialManager/AP_SerialManager.h>
28 : #include <GCS_MAVLink/GCS.h>
29 : #include <AP_HAL/AP_HAL.h>
30 : #include <AP_GPS/AP_GPS.h>
31 : #include <AP_Baro/AP_Baro.h>
32 : #include <AP_Compass/AP_Compass.h>
33 : #include <AP_InertialSensor/AP_InertialSensor.h>
34 :
35 : namespace
36 : {
37 : const float MINIMUM_INTERESTING_BAROMETER_CHANGE_p = 1.0f;
38 : const float MINIMUM_INTERESTING_TEMP_CHANGE_degc = 0.1f;
39 : const uint32_t BARO_UPDATE_TIMEOUT_ms = 100U;
40 : }
41 :
42 : extern const AP_HAL::HAL &hal;
43 :
44 3 : AP_ExternalAHRS_SensAItion::AP_ExternalAHRS_SensAItion(AP_ExternalAHRS *_frontend, AP_ExternalAHRS::state_t &_state) :
45 : AP_ExternalAHRS_backend(_frontend, _state),
46 3 : parser(AP_ExternalAHRS_SensAItion_Parser::ConfigMode::IMU)
47 : {
48 3 : ins_mode_enabled = option_is_set(AP_ExternalAHRS::OPTIONS::SENSAITION_INS);
49 :
50 3 : auto mode = ins_mode_enabled ?
51 : AP_ExternalAHRS_SensAItion_Parser::ConfigMode::INTERLEAVED_INS :
52 : AP_ExternalAHRS_SensAItion_Parser::ConfigMode::IMU;
53 :
54 3 : if (ins_mode_enabled) {
55 1 : parser = AP_ExternalAHRS_SensAItion_Parser(mode);
56 : }
57 :
58 3 : auto &sm = AP::serialmanager();
59 3 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
60 3 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
61 3 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
62 3 : if (!uart || baudrate == 0 || port_num == -1) {
63 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "KEBNI: Serial Port Not Found!");
64 0 : return;
65 : }
66 :
67 3 : if (ins_mode_enabled) {
68 1 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::IMU) |
69 : uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) |
70 : uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) |
71 : uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
72 : } else {
73 2 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::IMU));
74 : }
75 :
76 3 : if (!hal.scheduler->thread_create(
77 : FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_SensAItion::update_thread, void),
78 : "AHRS_SensAItion", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
79 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "KEBNI: Failed to create thread!");
80 : }
81 : }
82 :
83 10 : int8_t AP_ExternalAHRS_SensAItion::get_port() const
84 : {
85 10 : return uart ? port_num : -1;
86 : }
87 :
88 0 : const char* AP_ExternalAHRS_SensAItion::get_name() const
89 : {
90 0 : return "Kebni SensAItion";
91 : }
92 :
93 161837 : bool AP_ExternalAHRS_SensAItion::healthy() const
94 : {
95 161837 : const uint32_t now_ms = AP_HAL::millis();
96 :
97 161837 : WITH_SEMAPHORE(driver_state.semaphore);
98 :
99 161837 : if ((now_ms - driver_state.last_imu_pkt_ms) > 160) {
100 : // IMU is required at high rate for health
101 10 : return false;
102 : }
103 :
104 161827 : if (ins_mode_enabled) {
105 11403 : if ((now_ms - driver_state.last_ins_pkt_ms) > 400) {
106 : // INS packets must also have high enough rate
107 0 : return false;
108 : }
109 :
110 11403 : if (!(driver_state.last_sensor_valid & 0x01)) {
111 : // IMU not available
112 1 : return false;
113 : }
114 :
115 11402 : if (driver_state.last_gnss1_fix < 3) {
116 : // No 3D satellite fix
117 0 : return false;
118 : }
119 : }
120 :
121 161826 : return true;
122 161837 : }
123 :
124 28840 : bool AP_ExternalAHRS_SensAItion::initialised() const
125 : {
126 28840 : return setup_complete;
127 : }
128 :
129 132997 : bool AP_ExternalAHRS_SensAItion::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
130 : {
131 132997 : if (!healthy()) {
132 0 : hal.util->snprintf(failure_msg, failure_msg_len, "SensAItion Unhealthy");
133 0 : return false;
134 : }
135 :
136 132997 : if (ins_mode_enabled) {
137 : uint8_t last_alignment_status;
138 : {
139 10 : WITH_SEMAPHORE(driver_state.semaphore);
140 10 : last_alignment_status = driver_state.last_alignment_status;
141 10 : }
142 10 : if (last_alignment_status != 1) {
143 0 : hal.util->snprintf(failure_msg, failure_msg_len, "SensAItion Aligning");
144 0 : return false;
145 : }
146 : }
147 :
148 132997 : return true;
149 : }
150 :
151 8009 : void AP_ExternalAHRS_SensAItion::get_filter_status(nav_filter_status &status) const
152 : {
153 8009 : memset(&status, 0, sizeof(status));
154 :
155 8009 : status.flags.initalized = initialised();
156 :
157 8009 : if (healthy()) {
158 7998 : WITH_SEMAPHORE(driver_state.semaphore);
159 7998 : if (ins_mode_enabled && driver_state.last_alignment_status == 1) {
160 6216 : status.flags.attitude = true;
161 6216 : status.flags.horiz_pos_abs = true;
162 6216 : status.flags.vert_pos = true;
163 6216 : status.flags.horiz_vel = true;
164 6216 : status.flags.vert_vel = true;
165 6216 : status.flags.using_gps = true;
166 6216 : status.flags.horiz_pos_rel = true;
167 6216 : status.flags.pred_horiz_pos_abs = true;
168 6216 : status.flags.pred_horiz_pos_rel = true;
169 : }
170 7998 : }
171 8009 : }
172 :
173 8009 : bool AP_ExternalAHRS_SensAItion::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
174 : {
175 8009 : WITH_SEMAPHORE(driver_state.semaphore);
176 8009 : if (ins_mode_enabled && driver_state.last_alignment_status == 1) {
177 6216 : posVar = driver_state.last_h_pos_quality * pos_gate_scale;
178 6216 : velVar = driver_state.last_vel_quality * vel_gate_scale;
179 6216 : hgtVar = driver_state.last_v_pos_quality * hgt_gate_scale;
180 6216 : tasVar = 0; //not used
181 6216 : return true;
182 : }
183 :
184 1793 : return false;
185 8009 : }
186 :
187 10 : uint8_t AP_ExternalAHRS_SensAItion::num_gps_sensors() const
188 : {
189 10 : return ins_mode_enabled ? 1 : 0;
190 : }
191 :
192 : // ---------------------------------------------------------------------------
193 : // THREAD & PROFILER
194 : // ---------------------------------------------------------------------------
195 103928 : void AP_ExternalAHRS_SensAItion::update_thread()
196 : {
197 : while (true) {
198 103928 : if (!check_uart()) {
199 70719 : hal.scheduler->delay_microseconds(500);
200 : }
201 : }
202 : }
203 :
204 191076 : bool AP_ExternalAHRS_SensAItion::check_uart()
205 : {
206 191076 : WITH_SEMAPHORE(sem_handle);
207 :
208 191093 : if (!uart) {
209 0 : return false;
210 : }
211 :
212 191093 : if (!setup_complete) {
213 3 : uart->begin(baudrate);
214 3 : setup_complete = true;
215 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "KEBNI: INIT. Mode:%d Baud:%u",
216 : (int)ins_mode_enabled, (unsigned)baudrate);
217 : }
218 :
219 191093 : const auto nread = uart->read(buffer, sizeof(buffer));
220 191093 : if (nread <= 0) {
221 139853 : return false;
222 : }
223 :
224 51240 : const uint32_t now_ms = AP_HAL::millis();
225 :
226 51240 : AP_ExternalAHRS_SensAItion_Parser::Measurement meas;
227 51240 : bool found_measurement = false;
228 51240 : ssize_t parsed_bytes = 0;
229 111719 : while (parsed_bytes < nread) {
230 60479 : const size_t max_bytes_to_parse = nread - parsed_bytes;
231 60479 : parsed_bytes += parser.parse_stream(&buffer[parsed_bytes], max_bytes_to_parse, meas);
232 :
233 60479 : switch (meas.type) {
234 0 : case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::UNINITIALIZED:
235 : // The parser is done parsing the whole buffer, but didn't find any
236 : // more complete message. (We might be in the middle of a message
237 : // that will be finished later.)
238 0 : break;
239 58328 : case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::IMU:
240 58328 : handle_imu(meas, now_ms);
241 58328 : found_measurement = true;
242 58328 : break;
243 1075 : case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::AHRS:
244 1075 : handle_ahrs(meas, now_ms);
245 1075 : found_measurement = true;
246 1075 : break;
247 1076 : case AP_ExternalAHRS_SensAItion_Parser::MeasurementType::INS:
248 1076 : handle_ins(meas, now_ms);
249 1076 : found_measurement = true;
250 1076 : break;
251 : }
252 : }
253 :
254 51240 : return found_measurement;
255 191093 : }
256 :
257 58328 : void AP_ExternalAHRS_SensAItion::handle_imu(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms)
258 : {
259 : {
260 58328 : WITH_SEMAPHORE(state.sem);
261 58328 : state.accel = meas.acceleration_mss;
262 58328 : state.gyro = meas.angular_velocity_rads;
263 58328 : }
264 :
265 : // INS
266 58328 : ins.accel = meas.acceleration_mss;
267 58328 : ins.gyro = meas.angular_velocity_rads;
268 58328 : ins.temperature = meas.temperature_degc;
269 58328 : AP::ins().handle_external(ins);
270 :
271 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
272 : // COMPASS
273 58328 : mag.field = meas.magnetic_field_mgauss;
274 58328 : AP::compass().handle_external(mag);
275 : #endif
276 :
277 : #if AP_BARO_EXTERNALAHRS_ENABLED
278 58328 : bool baro_updated = false;
279 : #endif
280 :
281 : {
282 58328 : WITH_SEMAPHORE(driver_state.semaphore);
283 58328 : driver_state.last_imu_pkt_ms = now_ms;
284 :
285 : #if AP_BARO_EXTERNALAHRS_ENABLED
286 : // BARO
287 : // ArduPlane has an internal check that triggers an error if there are too many barometer
288 : // readings with the same value. At high sampling rates, we triggered that check because
289 : // the barometer is internally sampled at a lower rate. To avoid that, we only update the value
290 : // if it changes OR a certain minimum time has passed.
291 58328 : const bool pressure_changed = fabsf(baro.pressure_pa - meas.air_pressure_p) > MINIMUM_INTERESTING_BAROMETER_CHANGE_p;
292 58328 : const bool temp_changed = fabsf(baro.temperature - meas.temperature_degc) > MINIMUM_INTERESTING_TEMP_CHANGE_degc;
293 58328 : const bool timeout = now_ms > driver_state.last_baro_update_ms + BARO_UPDATE_TIMEOUT_ms;
294 :
295 58328 : if (pressure_changed || temp_changed || timeout) {
296 52428 : driver_state.last_baro_update_ms = now_ms;
297 52428 : baro.instance = 0;
298 52428 : baro.pressure_pa = meas.air_pressure_p;
299 52428 : baro.temperature = meas.temperature_degc;
300 52428 : baro_updated = true;
301 : }
302 : #endif
303 :
304 58328 : }
305 :
306 : #if AP_BARO_EXTERNALAHRS_ENABLED
307 : // Do the update after releasing the semaphore, for speed
308 58328 : if (baro_updated) {
309 52428 : AP::baro().handle_external(baro);
310 : }
311 : #endif
312 58328 : }
313 :
314 1075 : void AP_ExternalAHRS_SensAItion::handle_ahrs(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms)
315 : {
316 : {
317 1075 : WITH_SEMAPHORE(driver_state.semaphore);
318 1075 : driver_state.last_quat_pkt_ms = now_ms;
319 1075 : }
320 :
321 : {
322 1075 : WITH_SEMAPHORE(state.sem);
323 1075 : state.quat = meas.orientation;
324 1075 : state.have_quaternion = true;
325 1075 : }
326 1075 : }
327 :
328 1076 : void AP_ExternalAHRS_SensAItion::handle_ins(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms)
329 : {
330 : // STATE
331 : {
332 1076 : WITH_SEMAPHORE(state.sem);
333 2152 : state.location = Location(
334 1076 : meas.location.lat,
335 1076 : meas.location.lng,
336 1076 : meas.location.alt,
337 : Location::AltFrame::ABSOLUTE
338 : );
339 1076 : state.velocity = meas.velocity_ned;
340 1076 : state.have_location = true;
341 1076 : state.have_velocity = true;
342 1076 : state.last_location_update_us = AP_HAL::micros();
343 :
344 1076 : if (!state.have_origin && meas.alignment_status) {
345 2 : state.origin = Location(
346 1 : meas.location.lat,
347 1 : meas.location.lng,
348 1 : meas.location.alt,
349 : Location::AltFrame::ABSOLUTE
350 : );
351 1 : state.have_origin = true;
352 1 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "KEBNI: Origin Set.");
353 : }
354 1076 : }
355 :
356 : {
357 1076 : WITH_SEMAPHORE(driver_state.semaphore);
358 :
359 : // Local data
360 1076 : driver_state.last_ins_pkt_ms = now_ms;
361 1076 : driver_state.last_alignment_status = meas.alignment_status;
362 1076 : driver_state.last_sensor_valid = meas.sensor_valid;
363 1076 : driver_state.last_gnss1_fix = meas.gnss1_fix;
364 1076 : driver_state.last_gnss2_fix = meas.gnss2_fix;
365 1076 : driver_state.last_error_flags = meas.error_flags;
366 1076 : driver_state.last_h_pos_quality = meas.pos_accuracy.xy().length();
367 1076 : driver_state.last_v_pos_quality = meas.pos_accuracy.z;
368 1076 : driver_state.last_vel_quality = meas.vel_accuracy.length();
369 :
370 : // GPS
371 1076 : gps.horizontal_pos_accuracy = driver_state.last_h_pos_quality;
372 1076 : gps.vertical_pos_accuracy = driver_state.last_v_pos_quality;
373 1076 : }
374 :
375 : // GPS - continued
376 1076 : gps.gps_week = meas.gps_week;
377 1076 : gps.ms_tow = meas.time_itow_ms;
378 1076 : gps.fix_type = AP_GPS_FixType(meas.gnss1_fix);
379 1076 : gps.satellites_in_view = meas.num_sats_gnss1;
380 1076 : gps.horizontal_vel_accuracy = meas.vel_accuracy.xy().length();
381 1076 : gps.latitude = meas.location.lat;
382 1076 : gps.longitude = meas.location.lng;
383 :
384 : // Note: SensAItion reports altitude relative to WGS84, not MSL.
385 : // But we expect the user to reset the altitude to 0 at start,
386 : // so the absolute reference should not matter.
387 1076 : gps.msl_altitude = meas.location.alt;
388 1076 : gps.ned_vel_north = meas.velocity_ned.x;
389 1076 : gps.ned_vel_east = meas.velocity_ned.y;
390 1076 : gps.ned_vel_down = meas.velocity_ned.z;
391 :
392 : // 3. Estimate DOPs (Unitless) using assumed UERE of 3.0m
393 : // This answers "What is HDOP/VDOP?"
394 1076 : const float ASSUMED_UERE = 3.0f;
395 :
396 1076 : float est_hdop = gps.horizontal_pos_accuracy / ASSUMED_UERE;
397 1076 : float est_vdop = gps.vertical_pos_accuracy / ASSUMED_UERE;
398 :
399 : // 4. Sanity Clamping (DOP cannot be 0, and rarely < 0.6)
400 1076 : if (est_hdop < 0.7f) {
401 1076 : est_hdop = 0.7f;
402 : }
403 1076 : if (est_vdop < 0.7f) {
404 1076 : est_vdop = 0.7f;
405 : }
406 1076 : gps.hdop = est_hdop;
407 1076 : gps.vdop = est_vdop;
408 :
409 : // Handle
410 : uint8_t instance;
411 1076 : if (AP::gps().get_first_external_instance(instance)) {
412 1051 : AP::gps().handle_external(gps, instance);
413 : }
414 1076 : }
415 :
416 : #endif // AP_EXTERNAL_AHRS_SENSAITION_ENABLED
|
