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 : // Usage in SITL with hardware for debugging:
15 : // $ sim_vehicle.py -v Plane --console --map -DG
16 : // param set AHRS_EKF_TYPE 11
17 : // param set EAHRS_TYPE 6
18 : // For EAHRS as a GPS:
19 : // param set GPS1_TYPE 21
20 : // Configure GSOF 1, 49,50,70 on UDP port 44444, "UDP Mode" and "UDP Broadcast Transmit"
21 : // Consider setting EK3_SRC1_YAW to 2 on the bench...
22 :
23 : // Usage with NET parameters and ethernet in SITL with hardware:
24 : // param set NET_ENABLE 1
25 : // param set NET_P1_TYPE 2
26 : // # Set up AHRS input
27 : // param set NET_P1_PROTOCOL 36
28 : // param set SIM_GPS1_TYPE 0
29 : // param set NET_P1_PORT 44444
30 : // param set EAHRS_TYPE 6
31 : //
32 : // Usage with serial in SITL with hardware:
33 : // $ sim_vehicle.py -v Plane -A "--serial3=uart:/dev/ttyUSB0" --console --map -DG
34 : // param set SERIAL3_PROTOCOL 36
35 : // param set SERIAL3_BAUD 115200
36 : // param set EAHRS_TYPE 6
37 : // # ensure NET_* if off if you were using ethernet.
38 : // # To enable the EAHRS to provide GPS:
39 : // param set GPS2_TYPE 21
40 :
41 : // On most hardware, you must enable EAHRS:
42 : // ./waf configure --board Pixhawk6X --enable-AHRS_EXT
43 :
44 : // GPS ride-along with EARHS as the 2nd GPS
45 : // param set GPS_AUTO_SWITCH 0
46 : // param set GPS2_TYPE 21
47 : // param set GPS_PRIMARY 0
48 :
49 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
50 :
51 : #include "AP_ExternalAHRS_config.h"
52 :
53 : #if AP_EXTERNAL_AHRS_GSOF_ENABLED
54 :
55 : #include "AP_ExternalAHRS_GSOF.h"
56 : #include <AP_Baro/AP_Baro.h>
57 : #include <AP_Compass/AP_Compass.h>
58 : #include <AP_GPS/AP_GPS.h>
59 : #include <AP_HAL/utility/sparse-endian.h>
60 : #include <AP_InertialSensor/AP_InertialSensor.h>
61 : #include <GCS_MAVLink/GCS.h>
62 : #include <AP_Logger/AP_Logger.h>
63 : #include <AP_BoardConfig/AP_BoardConfig.h>
64 : #include <AP_HAL/utility/Socket.h>
65 :
66 : static const char* LOG_FMT = "%s ExternalAHRS: %s";
67 :
68 : extern const AP_HAL::HAL &hal;
69 :
70 0 : AP_ExternalAHRS_GSOF::AP_ExternalAHRS_GSOF(AP_ExternalAHRS *_frontend,
71 0 : AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
72 : {
73 0 : auto &sm = AP::serialmanager();
74 0 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
75 0 : if (uart == nullptr) {
76 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_FMT, get_name(), "no UART");
77 0 : return;
78 : }
79 0 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
80 0 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
81 :
82 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_GSOF::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
83 0 : AP_BoardConfig::allocation_error("GSOF ExternalAHRS failed to allocate ExternalAHRS update thread");
84 : }
85 :
86 : // Offer GPS even though it's a tightly coupled EKF.
87 0 : set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS));
88 : }
89 :
90 : // get serial port number for the uart
91 0 : int8_t AP_ExternalAHRS_GSOF::get_port(void) const
92 : {
93 0 : if (uart == nullptr) {
94 0 : return -1;
95 : }
96 0 : return port_num;
97 : };
98 :
99 0 : void AP_ExternalAHRS_GSOF::update_thread(void)
100 : {
101 : // TODO configure receiver to output expected data.
102 :
103 : #if AP_EXTERNAL_AHRS_GSOF_DEBUG_ENABLED
104 : auto last_debug = AP_HAL::millis();
105 : uint32_t pps = 0;
106 : #endif // AP_EXTERNAL_AHRS_GSOF_DEBUG_ENABLED
107 :
108 0 : uart->begin(baudrate);
109 :
110 : while (true) {
111 0 : const auto available = uart->available();
112 0 : if (available == 0) {
113 0 : hal.scheduler->delay_microseconds(100);
114 0 : continue;
115 : }
116 : uint8_t c;
117 0 : if (!uart->read(c)) {
118 0 : hal.scheduler->delay_microseconds(100);
119 0 : continue;
120 : }
121 :
122 0 : AP_GSOF::MsgTypes parsed {};
123 0 : const auto parse_res = parse(c, parsed);
124 0 : if (parse_res != PARSED_GSOF_DATA) {
125 0 : continue;
126 : }
127 :
128 0 : auto const now = AP_HAL::millis();
129 : #if AP_EXTERNAL_AHRS_GSOF_DEBUG_ENABLED
130 : pps++;
131 :
132 : if (now - last_debug > 1000) {
133 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
134 : "GSOF PPS: %lu, ins %u",
135 : static_cast<unsigned long>(pps),
136 : static_cast<uint16_t>(ins_full_nav.gnss_status));
137 : last_debug = now;
138 : pps = 0;
139 : }
140 : #endif // AP_EXTERNAL_AHRS_GSOF_DEBUG_ENABLED
141 0 : if (parsed.get(AP_GSOF::POS_TIME)) {
142 0 : last_pos_time_ms = now;
143 :
144 0 : gps_data.satellites_in_view = pos_time.num_sats;
145 : }
146 :
147 0 : if (parsed.get(AP_GSOF::INS_FULL_NAV)) {
148 0 : last_ins_full_nav_ms = now;
149 :
150 : // The Trimble PX-1 is a tightly coupled GNSS-INS.
151 : // Although generally we don't want to couple EKF's
152 : // by populating EKF data from a GPS into ArduPilot's GPS
153 : // which then goes to AP's EKF, this is the only approach.
154 : // Extensive flight testing indicates good performance,
155 : // and proper handling of failure of the PX-1 INS results
156 : // in proper ignoring of the GPS data.
157 0 : gps_data.gps_week = ins_full_nav.gps_week;
158 0 : gps_data.ms_tow = ins_full_nav.gps_time_ms;
159 0 : gps_data.ned_vel_north = ins_full_nav.vel_n;
160 0 : gps_data.ned_vel_east = ins_full_nav.vel_e;
161 0 : gps_data.ned_vel_down = ins_full_nav.vel_d;
162 0 : switch(ins_full_nav.gnss_status) {
163 0 : case AP_GSOF::GnssStatus::FIX_NOT_AVAILABLE:
164 0 : gps_data.fix_type = AP_GPS_FixType::NONE;
165 0 : break;
166 0 : case AP_GSOF::GnssStatus::GNSS_SPS_MODE:
167 0 : gps_data.fix_type = AP_GPS_FixType::FIX_3D;
168 0 : break;
169 0 : case AP_GSOF::GnssStatus::DGPS_SPS_MODE:
170 0 : gps_data.fix_type = AP_GPS_FixType::DGPS;
171 0 : break;
172 0 : case AP_GSOF::GnssStatus::GNSS_PPS_MODE:
173 0 : gps_data.fix_type = AP_GPS_FixType::PPP;
174 0 : break;
175 0 : case AP_GSOF::GnssStatus::FIXED_RTK_MODE:
176 0 : gps_data.fix_type = AP_GPS_FixType::RTK_FIXED;
177 0 : break;
178 0 : case AP_GSOF::GnssStatus::FLOAT_RTK_MODE:
179 0 : gps_data.fix_type = AP_GPS_FixType::RTK_FLOAT;
180 0 : break;
181 0 : case AP_GSOF::GnssStatus::DR_MODE:
182 0 : gps_data.fix_type = AP_GPS_FixType::NONE;
183 0 : break;
184 : }
185 0 : if (ins_full_nav.gnss_status != AP_GSOF::GnssStatus::FIX_NOT_AVAILABLE) {
186 : // If fix is unavailable, the lat/lon may be zero, so don't post the data.
187 : // To reproduce this condition, disconnect the GPS antenna and reboot the receiver.
188 0 : post_filter();
189 : }
190 : }
191 0 : if (parsed.get(AP_GSOF::INS_RMS)) {
192 0 : last_ins_rms_ms = now;
193 :
194 0 : gps_data.horizontal_pos_accuracy = Vector2d(ins_rms.pos_rms_n, ins_rms.pos_rms_e).length();
195 0 : gps_data.vertical_pos_accuracy = ins_rms.pos_rms_d;
196 0 : gps_data.horizontal_vel_accuracy = Vector2d(ins_rms.vel_rms_n, ins_rms.vel_rms_e).length();
197 : }
198 0 : if (parsed.get(AP_GSOF::LLH_MSL)) {
199 0 : last_llh_msl_ms = now;
200 :
201 0 : gps_data.longitude = static_cast<int32_t>(llh_msl.longitude * 1E7);
202 0 : gps_data.latitude = static_cast<int32_t>(llh_msl.latitude * 1E7);
203 0 : gps_data.msl_altitude = static_cast<int32_t>(llh_msl.altitude_msl * 1E2);
204 : }
205 :
206 0 : if (parsed.get(AP_GSOF::LLH_MSL) && parsed.get(AP_GSOF::INS_FULL_NAV)) {
207 0 : undulation = ins_full_nav.altitude - llh_msl.altitude_msl;
208 : }
209 :
210 0 : AP_GSOF::MsgTypes expected_gps {};
211 0 : expected_gps.set(AP_GSOF::POS_TIME);
212 0 : expected_gps.set(AP_GSOF::INS_FULL_NAV);
213 0 : expected_gps.set(AP_GSOF::INS_RMS);
214 0 : expected_gps.set(AP_GSOF::LLH_MSL);
215 : uint8_t instance;
216 0 : if (AP::gps().get_first_external_instance(instance) && parsed == expected_gps) {
217 0 : AP::gps().handle_external(gps_data, instance);
218 : }
219 0 : check_initialise_state();
220 0 : }
221 : }
222 :
223 0 : void AP_ExternalAHRS_GSOF::check_initialise_state(void)
224 : {
225 0 : const bool new_init_state = initialised();
226 : // Only send the message after fully booted up, otherwise it gets dropped.
227 0 : if (!last_init_state && new_init_state && AP_HAL::millis() > 5000) {
228 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "initialised.");
229 0 : last_init_state = new_init_state;
230 : }
231 0 : }
232 :
233 0 : void AP_ExternalAHRS_GSOF::post_filter() const
234 : {
235 0 : WITH_SEMAPHORE(state.sem);
236 0 : state.velocity = Vector3f{ins_full_nav.vel_n, ins_full_nav.vel_e, ins_full_nav.vel_d};
237 0 : state.have_velocity = true;
238 :
239 0 : state.location = Location(
240 0 : ins_full_nav.latitude * 1E7,
241 0 : ins_full_nav.longitude * 1E7,
242 0 : (ins_full_nav.altitude - undulation) * 1E2,
243 : Location::AltFrame::ABSOLUTE);
244 0 : state.have_location = true;
245 :
246 0 : state.quat.from_euler(
247 0 : radians(ins_full_nav.roll_deg),
248 0 : radians(ins_full_nav.pitch_deg),
249 0 : radians(ins_full_nav.heading_deg));
250 0 : state.have_quaternion = true;
251 :
252 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
253 0 : if (!state.location.initialised()) {
254 0 : AP_HAL::panic("Uninitialized location.");
255 : }
256 : #endif
257 :
258 0 : if (!state.have_origin && filter_healthy()) {
259 0 : state.origin = state.location;
260 0 : state.have_origin = true;
261 : }
262 :
263 0 : state.last_location_update_us = AP_HAL::micros();
264 0 : }
265 :
266 : // Get model/type name
267 0 : const char* AP_ExternalAHRS_GSOF::get_name() const
268 : {
269 0 : return "GSOF";
270 : }
271 :
272 0 : bool AP_ExternalAHRS_GSOF::healthy(void) const
273 : {
274 0 : return times_healthy() && filter_healthy();
275 : }
276 :
277 0 : bool AP_ExternalAHRS_GSOF::initialised(void) const
278 : {
279 0 : return last_pos_time_ms != 0 && last_ins_full_nav_ms != 0 && last_ins_rms_ms != 0 && last_llh_msl_ms != 0;
280 : }
281 :
282 0 : bool AP_ExternalAHRS_GSOF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
283 : {
284 0 : if (!initialised()) {
285 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "not initialised");
286 0 : return false;
287 : }
288 0 : if (!times_healthy()) {
289 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "data is stale");
290 0 : return false;
291 : }
292 0 : if (!filter_healthy()) {
293 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter is unhealthy");
294 0 : return false;
295 : }
296 0 : if (!healthy()) {
297 0 : hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "unhealthy");
298 0 : return false;
299 : }
300 :
301 0 : return true;
302 : }
303 :
304 0 : void AP_ExternalAHRS_GSOF::get_filter_status(nav_filter_status &status) const
305 : {
306 0 : memset(&status, 0, sizeof(status));
307 0 : status.flags.initalized = initialised();
308 0 : }
309 :
310 0 : bool AP_ExternalAHRS_GSOF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
311 : {
312 0 : velVar = Vector3<float>(
313 0 : ins_rms.vel_rms_n,
314 0 : ins_rms.vel_rms_e,
315 0 : ins_rms.vel_rms_d).length() * vel_gate_scale;
316 0 : posVar = Vector2<float>(
317 0 : ins_rms.pos_rms_n,
318 0 : ins_rms.pos_rms_e).length() * pos_gate_scale;
319 0 : hgtVar = ins_rms.pos_rms_d * hgt_gate_scale;
320 0 : tasVar = 0;
321 0 : return true;
322 : }
323 :
324 0 : bool AP_ExternalAHRS_GSOF::times_healthy() const
325 : {
326 0 : auto const now = AP_HAL::millis();
327 :
328 0 : auto const TIMES_FOS = 2.0;
329 :
330 : // All messages must be at minimum 5Hz to pass the AP_GPS 4hz rate as logged in dataflash GPA.Delta logs.
331 :
332 : // 5Hz = 200mS.
333 0 : auto const GSOF_1_EXPECTED_DELAY_MS = 200;
334 0 : auto const pos_time_healthy = now - last_pos_time_ms <= TIMES_FOS * GSOF_1_EXPECTED_DELAY_MS;
335 :
336 : // 50Hz = 20mS.
337 0 : auto const GSOF_49_EXPECTED_DELAY_MS = 20;
338 0 : auto const ins_full_nav_healthy = now - last_ins_full_nav_ms <= TIMES_FOS * GSOF_49_EXPECTED_DELAY_MS;
339 :
340 : // 5Hz = 200mS.
341 0 : auto const GSOF_50_EXPECTED_DELAY_MS = 200;
342 0 : auto const ins_rms_healthy = now - last_ins_rms_ms <= TIMES_FOS * GSOF_50_EXPECTED_DELAY_MS;
343 :
344 : // 5Hz = 200mS.
345 0 : auto const GSOF_70_EXPECTED_DELAY_MS = 200;
346 0 : auto const llh_msl_healthy = now - last_llh_msl_ms <= TIMES_FOS * GSOF_70_EXPECTED_DELAY_MS;
347 :
348 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
349 0 : if (!pos_time_healthy) {
350 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: GSOF pos time delayed by %f ms", get_name(), float(now - last_pos_time_ms));
351 : }
352 0 : if (!ins_full_nav_healthy) {
353 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: INS Full nav delayed by %f ms", get_name(), float(now - last_ins_full_nav_ms));
354 : }
355 0 : if (!ins_rms_healthy) {
356 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: INS rms delayed by %f ms", get_name(), float(now - last_ins_rms_ms));
357 : }
358 0 : if (!llh_msl_healthy) {
359 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: LLH MSL delayed by %f ms", get_name(), float(now - last_llh_msl_ms));
360 : }
361 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
362 :
363 0 : return pos_time_healthy && ins_full_nav_healthy && ins_rms_healthy && llh_msl_healthy;
364 : }
365 :
366 0 : bool AP_ExternalAHRS_GSOF::filter_healthy() const
367 : {
368 : // TODO get the right threshold from Trimble for arming vs in flight.
369 : // For now, assume an aligned IMU is sufficient for flight.
370 0 : auto const imu_alignment_healthy = (
371 0 : ins_rms.imu_alignment_status == ImuAlignmentStatus::DEGRADED ||
372 0 : ins_rms.imu_alignment_status == ImuAlignmentStatus::ALIGNED ||
373 0 : ins_rms.imu_alignment_status == ImuAlignmentStatus::FULL_NAV
374 : );
375 :
376 0 : auto const gnss_healthy = ins_rms.gnss_status != GnssStatus::FIX_NOT_AVAILABLE;
377 :
378 : // TODO check RMS errors.
379 0 : return imu_alignment_healthy && gnss_healthy;
380 : }
381 :
382 : #endif // AP_EXTERNAL_AHRS_GSOF_ENABLED
|
