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 serial connected AHRS systems
17 : */
18 :
19 : #include "AP_ExternalAHRS_config.h"
20 :
21 : #if AP_EXTERNAL_AHRS_ENABLED
22 :
23 : #include "AP_ExternalAHRS.h"
24 : #include "AP_ExternalAHRS_backend.h"
25 : #include "AP_ExternalAHRS_Aeron_plx.h"
26 : #include "AP_ExternalAHRS_VectorNav.h"
27 : #include "AP_ExternalAHRS_MicroStrain5.h"
28 : #include "AP_ExternalAHRS_MicroStrain7.h"
29 : #include "AP_ExternalAHRS_InertialLabs.h"
30 : #include "AP_ExternalAHRS_SBG.h"
31 : #include "AP_ExternalAHRS_GSOF.h"
32 : #include "AP_ExternalAHRS_SensAItion.h"
33 :
34 : #include <GCS_MAVLink/GCS.h>
35 : #include <AP_AHRS/AP_AHRS.h>
36 : #include <AP_GPS/AP_GPS.h>
37 : #include <AP_Logger/AP_Logger.h>
38 :
39 : extern const AP_HAL::HAL &hal;
40 :
41 : AP_ExternalAHRS *AP_ExternalAHRS::_singleton;
42 :
43 : // constructor
44 2098 : AP_ExternalAHRS::AP_ExternalAHRS()
45 : {
46 2098 : AP_Param::setup_object_defaults(this, var_info);
47 2098 : _singleton = this;
48 2098 : if (rate.get() < 50) {
49 : // min 50Hz
50 0 : rate.set(50);
51 : }
52 2098 : }
53 :
54 : #ifndef HAL_EXTERNAL_AHRS_DEFAULT
55 : #define HAL_EXTERNAL_AHRS_DEFAULT 0
56 : #endif
57 :
58 :
59 : // table of user settable parameters
60 : const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
61 :
62 : // @Param: _TYPE
63 : // @DisplayName: AHRS type
64 : // @Description: Type of AHRS device
65 : // @Values: 0:None,1:VectorNav,2:MicroStrain5,5:InertialLabs,6:Trimble GSOF,7:MicroStrain7,8:SBG,10:Aeron,11:SensAItion
66 : // @User: Standard
67 : AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE),
68 :
69 : // @Param: _RATE
70 : // @DisplayName: AHRS data rate
71 : // @Description: Requested rate for AHRS device
72 : // @Units: Hz
73 : // @User: Standard
74 : AP_GROUPINFO("_RATE", 2, AP_ExternalAHRS, rate, 50),
75 :
76 : // @Param: _OPTIONS
77 : // @DisplayName: External AHRS options
78 : // @Description: External AHRS options bitmask
79 : // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag, 1:SBG uses EKF as GNSS, 2:SensAItion used as AHRS
80 : // @User: Standard
81 : AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0),
82 :
83 : // @Param: _SENSORS
84 : // @DisplayName: External AHRS sensors
85 : // @Description: External AHRS sensors bitmask
86 : // @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass
87 : // @User: Advanced
88 : AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF),
89 :
90 : // @Param: _LOG_RATE
91 : // @DisplayName: AHRS logging rate
92 : // @Description: Logging rate for EAHRS devices
93 : // @Units: Hz
94 : // @User: Standard
95 : AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10),
96 :
97 : AP_GROUPEND
98 : };
99 :
100 :
101 2094 : void AP_ExternalAHRS::init(void)
102 : {
103 2094 : if (rate.get() < 50) {
104 : // min 50Hz
105 0 : rate.set(50);
106 : }
107 :
108 2094 : switch (DevType(devtype)) {
109 2032 : case DevType::None:
110 : // nothing to do
111 2032 : return;
112 :
113 : #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
114 12 : case DevType::VecNav:
115 12 : backend = NEW_NOTHROW AP_ExternalAHRS_VectorNav(this, state);
116 12 : return;
117 : #endif
118 :
119 : #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
120 11 : case DevType::MicroStrain5:
121 11 : backend = NEW_NOTHROW AP_ExternalAHRS_MicroStrain5(this, state);
122 11 : return;
123 : #endif
124 :
125 : #if AP_EXTERNAL_AHRS_GSOF_ENABLED
126 0 : case DevType::GSOF:
127 0 : backend = NEW_NOTHROW AP_ExternalAHRS_GSOF(this, state);
128 0 : return;
129 : #endif
130 :
131 : #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
132 14 : case DevType::MicroStrain7:
133 14 : backend = NEW_NOTHROW AP_ExternalAHRS_MicroStrain7(this, state);
134 14 : return;
135 : #endif
136 :
137 : #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
138 11 : case DevType::InertialLabs:
139 11 : backend = NEW_NOTHROW AP_ExternalAHRS_InertialLabs(this, state);
140 11 : return;
141 : #endif
142 : #if AP_EXTERNAL_AHRS_SENSAITION_ENABLED
143 3 : case DevType::SensAItion:
144 3 : backend = NEW_NOTHROW AP_ExternalAHRS_SensAItion(this, state);
145 3 : return;
146 : #endif
147 :
148 : #if AP_EXTERNAL_AHRS_SBG_ENABLED
149 0 : case DevType::SBG:
150 0 : backend = NEW_NOTHROW AP_ExternalAHRS_SBG(this, state);
151 0 : return;
152 : #endif // AP_EXTERNAL_AHRS_SBG_ENABLED
153 :
154 : #if AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
155 11 : case DevType::Aeron:
156 11 : backend = NEW_NOTHROW AP_ExternalAHRS_Aeron_plx(this, state);
157 11 : return;
158 : #endif // AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
159 :
160 : }
161 :
162 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype));
163 : }
164 :
165 42359120 : bool AP_ExternalAHRS::enabled() const
166 : {
167 42359120 : return DevType(devtype) != DevType::None;
168 : }
169 :
170 : // get serial port number for the uart, or -1 if not applicable
171 7290 : int8_t AP_ExternalAHRS::get_port(AvailableSensor sensor) const
172 : {
173 7290 : if (!backend || !has_sensor(sensor)) {
174 7101 : return -1;
175 : }
176 189 : return backend->get_port();
177 : };
178 :
179 : // accessors for AP_AHRS
180 41686716 : bool AP_ExternalAHRS::healthy(void) const
181 : {
182 41686716 : return backend && backend->healthy();
183 : }
184 :
185 41686716 : bool AP_ExternalAHRS::initialised(void) const
186 : {
187 41686716 : return backend && backend->initialised();
188 : }
189 :
190 41686716 : bool AP_ExternalAHRS::get_quaternion(Quaternion &quat)
191 : {
192 41686716 : if (state.have_quaternion) {
193 59493 : WITH_SEMAPHORE(state.sem);
194 59493 : quat = state.quat;
195 59493 : return true;
196 59493 : }
197 41627223 : return false;
198 : }
199 :
200 236255 : bool AP_ExternalAHRS::get_origin(Location &loc)
201 : {
202 236255 : if (state.have_origin) {
203 230917 : WITH_SEMAPHORE(state.sem);
204 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
205 230917 : if (!state.origin.initialised()) {
206 0 : AP_HAL::panic("Uninitialized origin in AP_ExternalAHRS.");
207 : }
208 : #endif
209 230917 : loc = state.origin;
210 230917 : return true;
211 230917 : }
212 5338 : return false;
213 : }
214 :
215 842 : bool AP_ExternalAHRS::set_origin(const Location &loc)
216 : {
217 842 : WITH_SEMAPHORE(state.sem);
218 842 : if (state.have_origin) {
219 7 : return false;
220 : }
221 835 : state.origin = loc;
222 835 : state.have_origin = true;
223 835 : return true;
224 842 : }
225 :
226 59493 : bool AP_ExternalAHRS::get_location(Location &loc)
227 : {
228 59493 : if (!state.have_location) {
229 0 : return false;
230 : }
231 59493 : WITH_SEMAPHORE(state.sem);
232 59493 : loc = state.location;
233 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
234 59493 : if (!loc.initialised()) {
235 0 : AP_HAL::panic("Uninitialized location in AP_ExternalAHRS.");
236 : }
237 : #endif
238 :
239 59493 : if (state.last_location_update_us != 0 &&
240 59493 : state.have_velocity) {
241 : // extrapolate position based on velocity to cope with slow backends
242 59493 : const float dt = (AP_HAL::micros() - state.last_location_update_us)*1.0e-6;
243 59493 : if (dt < 1) {
244 : // only extrapolate for 1s max
245 59493 : Vector3p ofs = state.velocity.topostype();
246 59493 : ofs *= dt;
247 59493 : loc.offset(ofs);
248 : }
249 : }
250 :
251 59493 : return true;
252 59493 : }
253 :
254 59493 : Vector2f AP_ExternalAHRS::get_groundspeed_vector()
255 : {
256 59493 : WITH_SEMAPHORE(state.sem);
257 59493 : Vector2f vec{state.velocity.x, state.velocity.y};
258 118986 : return vec;
259 59493 : }
260 :
261 59493 : bool AP_ExternalAHRS::get_velocity_NED(Vector3f &vel)
262 : {
263 59493 : if (!state.have_velocity) {
264 0 : return false;
265 : }
266 59493 : WITH_SEMAPHORE(state.sem);
267 59493 : vel = state.velocity;
268 59493 : return true;
269 59493 : }
270 :
271 59493 : bool AP_ExternalAHRS::get_speed_down(float &speedD)
272 : {
273 59493 : if (!state.have_velocity) {
274 0 : return false;
275 : }
276 59493 : WITH_SEMAPHORE(state.sem);
277 59493 : speedD = state.velocity.z;
278 59493 : return true;
279 59493 : }
280 :
281 133284 : bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
282 : {
283 133284 : if (backend == nullptr) {
284 2 : hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: Invalid backend");
285 2 : return false;
286 : }
287 133282 : if (!backend->pre_arm_check(failure_msg, failure_msg_len)) {
288 0 : return false;
289 : }
290 : // Verify the user has configured the GPS to accept EAHRS data.
291 133282 : if (has_sensor(AvailableSensor::GPS)) {
292 295 : const auto eahrs_gps_sensors = backend->num_gps_sensors();
293 :
294 295 : const auto &gps = AP::gps();
295 295 : uint8_t n_configured_eahrs_gps = 0;
296 1180 : for (uint8_t i = 0; i < GPS_MAX_INSTANCES; ++i) {
297 885 : const auto gps_type = gps.get_type(i);
298 885 : if (gps_type == AP_GPS::GPS_TYPE_EXTERNAL_AHRS) {
299 270 : n_configured_eahrs_gps++;
300 : }
301 : }
302 :
303 : // Once AP supports at least 3 GPS's, change to == and remove the second condition.
304 : // At that point, enforce that all GPS's in EAHRS can report to AP_GPS.
305 295 : if (n_configured_eahrs_gps < 1 && eahrs_gps_sensors >= 1) {
306 25 : hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: Incorrect number of GPS sensors configured for EAHRS");
307 25 : return false;
308 : }
309 : }
310 133257 : AP_AHRS &ahrs = AP::ahrs();
311 133257 : if (ahrs.configured_ekf_type() == AP_AHRS::EKFType::EXTERNAL) {
312 : // when using EAHRS as the EKF source, we must have a valid position origin
313 133206 : if (!state.have_origin) {
314 8 : hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin");
315 8 : return false;
316 : }
317 : }
318 133249 : return true;
319 : }
320 :
321 : /*
322 : get filter status
323 : */
324 73219 : void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const
325 : {
326 73219 : status = {};
327 73219 : if (backend) {
328 72971 : backend->get_filter_status(status);
329 : }
330 73219 : }
331 :
332 : /*
333 : get estimated variances, return false if not implemented
334 : */
335 59493 : bool AP_ExternalAHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
336 : {
337 59493 : if (backend != nullptr) {
338 59493 : return backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar);
339 : }
340 0 : return false;
341 : }
342 :
343 59493 : bool AP_ExternalAHRS::get_gyro(Vector3f &gyro)
344 : {
345 59493 : WITH_SEMAPHORE(state.sem);
346 59493 : if (!has_sensor(AvailableSensor::IMU)) {
347 38868 : return false;
348 : }
349 20625 : gyro = state.gyro;
350 20625 : return true;
351 59493 : }
352 :
353 59493 : bool AP_ExternalAHRS::get_accel(Vector3f &accel)
354 : {
355 59493 : WITH_SEMAPHORE(state.sem);
356 59493 : if (!has_sensor(AvailableSensor::IMU)) {
357 38868 : return false;
358 : }
359 20625 : accel = state.accel;
360 20625 : return true;
361 59493 : }
362 :
363 42249549 : void AP_ExternalAHRS::update(void)
364 : {
365 42249549 : if (backend) {
366 637981 : backend->update();
367 : }
368 :
369 42249549 : WITH_SEMAPHORE(state.sem);
370 : #if HAL_LOGGING_ENABLED
371 42249549 : const uint32_t now_ms = AP_HAL::millis();
372 42249549 : if (enabled() && log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {
373 13726 : last_log_ms = now_ms;
374 :
375 : // @LoggerMessage: EAHR
376 : // @Description: External AHRS data
377 : // @Field: TimeUS: Time since system startup
378 : // @Field: Roll: euler roll
379 : // @Field: Pitch: euler pitch
380 : // @Field: Yaw: euler yaw
381 : // @Field: VN: velocity north
382 : // @Field: VE: velocity east
383 : // @Field: VD: velocity down
384 : // @Field: Lat: latitude
385 : // @Field: Lon: longitude
386 : // @Field: Alt: altitude AMSL
387 : // @Field: Flg: nav status flags
388 :
389 : float roll, pitch, yaw;
390 13726 : state.quat.to_euler(roll, pitch, yaw);
391 13726 : nav_filter_status filterStatus {};
392 13726 : get_filter_status(filterStatus);
393 :
394 27452 : AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg",
395 : "sdddnnnDUm-",
396 : "F000000GG0-",
397 : "QffffffLLfI",
398 : AP_HAL::micros64(),
399 13726 : degrees(roll), degrees(pitch), degrees(yaw),
400 13726 : state.velocity.x, state.velocity.y, state.velocity.z,
401 13726 : state.location.lat, state.location.lng, state.location.alt*0.01,
402 : filterStatus.value);
403 :
404 : // @LoggerMessage: EAHV
405 : // @Description: External AHRS variances
406 : // @Field: TimeUS: Time since system startup
407 : // @Field: Vel: velocity variance
408 : // @Field: Pos: position variance
409 : // @Field: Hgt: height variance
410 : // @Field: MagX: magnetic variance, X
411 : // @Field: MagY: magnetic variance, Y
412 : // @Field: MagZ: magnetic variance, Z
413 : // @Field: TAS: true airspeed variance
414 :
415 : float velVar, posVar, hgtVar, tasVar;
416 13726 : Vector3f magVar;
417 13726 : if (backend != nullptr && backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar)) {
418 23370 : AP::logger().WriteStreaming("EAHV", "TimeUS,Vel,Pos,Hgt,MagX,MagY,MagZ,TAS",
419 : "Qfffffff",
420 : AP_HAL::micros64(),
421 : velVar, posVar, hgtVar,
422 11685 : magVar.x, magVar.y, magVar.z,
423 : tasVar);
424 : }
425 : }
426 : #endif // HAL_LOGGING_ENABLED
427 42249549 : }
428 :
429 : // Get model/type name
430 0 : const char* AP_ExternalAHRS::get_name() const
431 : {
432 0 : if (backend) {
433 0 : return backend->get_name();
434 : }
435 0 : return nullptr;
436 : }
437 :
438 : namespace AP {
439 :
440 84741575 : AP_ExternalAHRS &externalAHRS()
441 : {
442 84741575 : return *AP_ExternalAHRS::get_singleton();
443 : }
444 :
445 : };
446 :
447 : #endif // AP_EXTERNAL_AHRS_ENABLED
448 :
|
