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 CX5/GX5-45 serially connected AHRS Systems
15 : */
16 :
17 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
18 :
19 : #include "AP_ExternalAHRS_config.h"
20 :
21 : #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
22 :
23 : #include "AP_ExternalAHRS_MicroStrain5.h"
24 : #include "AP_Compass/AP_Compass_config.h"
25 : #include <AP_Baro/AP_Baro.h>
26 : #include <AP_Compass/AP_Compass.h>
27 : #include <AP_GPS/AP_GPS.h>
28 : #include <AP_HAL/utility/sparse-endian.h>
29 : #include <AP_InertialSensor/AP_InertialSensor.h>
30 : #include <GCS_MAVLink/GCS.h>
31 : #include <AP_Logger/AP_Logger.h>
32 : #include <AP_BoardConfig/AP_BoardConfig.h>
33 : #include <AP_SerialManager/AP_SerialManager.h>
34 :
35 : extern const AP_HAL::HAL &hal;
36 :
37 : static constexpr uint8_t gnss_instance = 0;
38 :
39 11 : AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_frontend,
40 11 : AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
41 : {
42 11 : auto &sm = AP::serialmanager();
43 11 : uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
44 :
45 11 : baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
46 11 : port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
47 :
48 11 : if (!uart) {
49 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MicroStrain5 ExternalAHRS no UART");
50 0 : return;
51 : }
52 :
53 11 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain5::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
54 0 : AP_BoardConfig::allocation_error("MicroStrain5 failed to allocate ExternalAHRS update thread");
55 : }
56 :
57 11 : hal.scheduler->delay(5000);
58 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain5 ExternalAHRS initialised");
59 : }
60 :
61 11 : void AP_ExternalAHRS_MicroStrain5::update_thread(void)
62 : {
63 11 : if (!port_open) {
64 11 : port_open = true;
65 11 : uart->begin(baudrate);
66 : }
67 :
68 : while (true) {
69 58544 : build_packet();
70 58544 : hal.scheduler->delay_microseconds(100);
71 : }
72 : }
73 :
74 :
75 :
76 : // Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
77 242871 : void AP_ExternalAHRS_MicroStrain5::build_packet()
78 : {
79 242871 : if (uart == nullptr) {
80 0 : return;
81 : }
82 :
83 242871 : WITH_SEMAPHORE(sem);
84 242938 : uint32_t nbytes = MIN(uart->available(), 2048u);
85 1429358 : while (nbytes--> 0) {
86 : uint8_t b;
87 1186420 : if (!uart->read(b)) {
88 0 : break;
89 : }
90 : DescriptorSet descriptor;
91 1186420 : if (handle_byte(b, descriptor)) {
92 15901 : switch (descriptor) {
93 7645 : case DescriptorSet::IMUData:
94 7645 : post_imu();
95 7645 : break;
96 611 : case DescriptorSet::GNSSData:
97 : case DescriptorSet::GNSSRecv1:
98 : case DescriptorSet::GNSSRecv2:
99 611 : break;
100 7645 : case DescriptorSet::FilterData:
101 7645 : post_filter();
102 7645 : break;
103 0 : case DescriptorSet::BaseCommand:
104 : case DescriptorSet::DMCommand:
105 : case DescriptorSet::SystemCommand:
106 0 : break;
107 : }
108 : }
109 : }
110 242938 : }
111 :
112 :
113 :
114 : // Posts data from an imu packet to `state` and `handle_external` methods
115 7645 : void AP_ExternalAHRS_MicroStrain5::post_imu() const
116 : {
117 : {
118 7645 : WITH_SEMAPHORE(state.sem);
119 7645 : state.accel = imu_data.accel;
120 7645 : state.gyro = imu_data.gyro;
121 :
122 7645 : state.quat = imu_data.quat;
123 7645 : state.have_quaternion = true;
124 7645 : }
125 :
126 : {
127 7645 : AP_ExternalAHRS::ins_data_message_t ins {
128 : accel: imu_data.accel,
129 : gyro: imu_data.gyro,
130 : temperature: -300
131 7645 : };
132 7645 : AP::ins().handle_external(ins);
133 : }
134 :
135 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
136 : {
137 : AP_ExternalAHRS::mag_data_message_t mag {
138 : field: imu_data.mag
139 7645 : };
140 7645 : AP::compass().handle_external(mag);
141 : }
142 : #endif
143 :
144 : #if AP_BARO_EXTERNALAHRS_ENABLED
145 : {
146 7645 : const AP_ExternalAHRS::baro_data_message_t baro {
147 : instance: 0,
148 7645 : pressure_pa: imu_data.pressure,
149 : // setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by MicroStrain
150 : temperature: 25,
151 7645 : };
152 7645 : AP::baro().handle_external(baro);
153 : }
154 : #endif
155 7645 : }
156 :
157 7645 : void AP_ExternalAHRS_MicroStrain5::post_filter() const
158 : {
159 : {
160 7645 : WITH_SEMAPHORE(state.sem);
161 7645 : state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
162 7645 : state.have_velocity = true;
163 :
164 7645 : state.location = Location{filter_data.lat, filter_data.lon, gnss_data[gnss_instance].msl_altitude, Location::AltFrame::ABSOLUTE};
165 7645 : state.have_location = true;
166 7645 : state.last_location_update_us = AP_HAL::micros();
167 7645 : }
168 :
169 : AP_ExternalAHRS::gps_data_message_t gps {
170 7645 : gps_week: filter_data.week,
171 7645 : ms_tow: filter_data.tow_ms,
172 7645 : fix_type: AP_GPS_FixType(gnss_data[gnss_instance].fix_type),
173 7645 : satellites_in_view: gnss_data[gnss_instance].satellites,
174 :
175 7645 : horizontal_pos_accuracy: gnss_data[gnss_instance].horizontal_position_accuracy,
176 7645 : vertical_pos_accuracy: gnss_data[gnss_instance].vertical_position_accuracy,
177 7645 : horizontal_vel_accuracy: gnss_data[gnss_instance].speed_accuracy,
178 :
179 7645 : hdop: gnss_data[gnss_instance].hdop,
180 7645 : vdop: gnss_data[gnss_instance].vdop,
181 :
182 7645 : longitude: filter_data.lon,
183 7645 : latitude: filter_data.lat,
184 7645 : msl_altitude: gnss_data[gnss_instance].msl_altitude,
185 :
186 7645 : ned_vel_north: filter_data.ned_velocity_north,
187 7645 : ned_vel_east: filter_data.ned_velocity_east,
188 7645 : ned_vel_down: filter_data.ned_velocity_down,
189 7645 : };
190 :
191 7645 : if (gps.fix_type >= AP_GPS_FixType::FIX_3D && !state.have_origin) {
192 11 : WITH_SEMAPHORE(state.sem);
193 22 : state.origin = Location{int32_t(filter_data.lat),
194 11 : int32_t(filter_data.lon),
195 11 : int32_t(gnss_data[gnss_instance].msl_altitude),
196 : Location::AltFrame::ABSOLUTE};
197 11 : state.have_origin = true;
198 11 : }
199 :
200 : uint8_t gps_instance;
201 7645 : if (AP::gps().get_first_external_instance(gps_instance)) {
202 4850 : AP::gps().handle_external(gps, gps_instance);
203 : }
204 7645 : }
205 :
206 34 : int8_t AP_ExternalAHRS_MicroStrain5::get_port(void) const
207 : {
208 34 : if (!uart) {
209 0 : return -1;
210 : }
211 34 : return port_num;
212 : };
213 :
214 : // Get model/type name
215 0 : const char* AP_ExternalAHRS_MicroStrain5::get_name() const
216 : {
217 0 : return "MicroStrain5";
218 : }
219 :
220 14948 : bool AP_ExternalAHRS_MicroStrain5::healthy(void) const
221 : {
222 14948 : uint32_t now = AP_HAL::millis();
223 14948 : return (now - last_imu_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500);
224 : }
225 :
226 6374 : bool AP_ExternalAHRS_MicroStrain5::initialised(void) const
227 : {
228 6374 : return last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
229 : }
230 :
231 50 : bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
232 : {
233 50 : if (!healthy()) {
234 0 : hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 unhealthy");
235 0 : return false;
236 : }
237 50 : if (gnss_data[gnss_instance].fix_type < AP_GPS_FixType::FIX_3D) {
238 0 : hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 no GPS lock");
239 0 : return false;
240 : }
241 50 : if (filter_status.state != 0x02) {
242 0 : hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 filter not running");
243 0 : return false;
244 : }
245 :
246 50 : return true;
247 : }
248 :
249 8524 : void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) const
250 : {
251 8524 : memset(&status, 0, sizeof(status));
252 8524 : if (last_imu_pkt != 0 && last_gps_pkt != 0) {
253 8524 : status.flags.initalized = true;
254 : }
255 8524 : if (healthy() && last_imu_pkt != 0) {
256 8524 : status.flags.attitude = true;
257 8524 : status.flags.vert_vel = true;
258 8524 : status.flags.vert_pos = true;
259 :
260 8524 : if (gnss_data[gnss_instance].fix_type >= AP_GPS_FixType::FIX_3D) {
261 8524 : status.flags.horiz_vel = true;
262 8524 : status.flags.horiz_pos_rel = true;
263 8524 : status.flags.horiz_pos_abs = true;
264 8524 : status.flags.pred_horiz_pos_rel = true;
265 8524 : status.flags.pred_horiz_pos_abs = true;
266 8524 : status.flags.using_gps = true;
267 : }
268 : }
269 8524 : }
270 :
271 : // get variances
272 8524 : bool AP_ExternalAHRS_MicroStrain5::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
273 : {
274 8524 : velVar = gnss_data[gnss_instance].speed_accuracy * vel_gate_scale;
275 8524 : posVar = gnss_data[gnss_instance].horizontal_position_accuracy * pos_gate_scale;
276 8524 : hgtVar = gnss_data[gnss_instance].vertical_position_accuracy * hgt_gate_scale;
277 8524 : tasVar = 0;
278 8524 : return true;
279 : }
280 :
281 :
282 : #endif // AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
