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 InertialLabs INS system
17 : */
18 :
19 : #pragma once
20 :
21 : #include "AP_ExternalAHRS_config.h"
22 :
23 : #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
24 :
25 : #include "AP_ExternalAHRS_backend.h"
26 :
27 : class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
28 :
29 : public:
30 : AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
31 :
32 : // get serial port number, -1 for not enabled
33 : int8_t get_port(void) const override;
34 :
35 : // accessors for AP_AHRS
36 : bool healthy(void) const override;
37 : bool initialised(void) const override;
38 : bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
39 : void get_filter_status(nav_filter_status &status) const override;
40 : bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
41 :
42 : // check for new data
43 8163 : void update() override {
44 8163 : check_uart();
45 8163 : }
46 :
47 : // Get model/type name
48 0 : const char* get_name() const override {
49 0 : return "ILabs";
50 : }
51 :
52 : enum class MessageType : uint8_t {
53 : GPS_INS_TIME_MS = 0x01,
54 : GPS_WEEK = 0x3C,
55 : ACCEL_DATA_HR = 0x23,
56 : GYRO_DATA_HR = 0x21,
57 : BARO_DATA = 0x25,
58 : MAG_DATA = 0x24,
59 : ORIENTATION_ANGLES = 0x07,
60 : VELOCITIES = 0x12,
61 : POSITION = 0x10,
62 : KF_VEL_COVARIANCE = 0x58,
63 : KF_POS_COVARIANCE = 0x57,
64 : UNIT_STATUS = 0x53,
65 : GNSS_EXTENDED_INFO = 0x4A,
66 : NUM_SATS = 0x3B,
67 : GNSS_POSITION = 0x30,
68 : GNSS_VEL_TRACK = 0x32,
69 : GNSS_POS_TIMESTAMP = 0x3E,
70 : GNSS_INFO_SHORT = 0x36,
71 : GNSS_NEW_DATA = 0x41,
72 : GNSS_JAM_STATUS = 0xC0,
73 : DIFFERENTIAL_PRESSURE = 0x28,
74 : TRUE_AIRSPEED = 0x86,
75 : WIND_SPEED = 0x8A,
76 : AIR_DATA_STATUS = 0x8D,
77 : SUPPLY_VOLTAGE = 0x50,
78 : TEMPERATURE = 0x52,
79 : UNIT_STATUS2 = 0x5A,
80 : GNSS_ANGLES = 0x33,
81 : GNSS_ANGLE_POS_TYPE = 0x3A,
82 : GNSS_HEADING_TIMESTAMP = 0x40,
83 : GNSS_DOP = 0x42,
84 : INS_SOLUTION_STATUS = 0x54,
85 : };
86 :
87 : /*
88 : packets consist of:
89 : ILabsHeader
90 : list of MessageType
91 : sequence of ILabsData
92 : checksum
93 : */
94 : struct PACKED ILabsHeader {
95 : uint16_t magic; // 0x55AA
96 : uint8_t msg_type; // always 1 for INS data
97 : uint8_t msg_id; // always 0x95
98 : uint16_t msg_len; // msg_len+2 is total packet length
99 : };
100 :
101 : struct PACKED vec3_16_t {
102 : int16_t x,y,z;
103 70668 : Vector3f tofloat(void) {
104 70668 : return Vector3f(x,y,z);
105 : }
106 : };
107 : struct PACKED vec3_32_t {
108 : int32_t x,y,z;
109 106002 : Vector3f tofloat(void) {
110 106002 : return Vector3f(x,y,z);
111 : }
112 : };
113 : struct PACKED vec3_u8_t {
114 : uint8_t x,y,z;
115 35334 : Vector3f tofloat(void) {
116 35334 : return Vector3f(x,y,z);
117 : }
118 : };
119 : struct PACKED vec3_u16_t {
120 : uint16_t x,y,z;
121 35334 : Vector3f tofloat(void) {
122 35334 : return Vector3f(x,y,z);
123 : }
124 : };
125 :
126 : struct gnss_extended_info_t {
127 : uint8_t fix_type;
128 : uint8_t spoofing_status;
129 : };
130 :
131 : struct gnss_info_short_t {
132 : uint8_t info1;
133 : uint8_t info2;
134 : };
135 :
136 : union PACKED ILabsData {
137 : uint32_t gnss_time_ms; // ms since start of GNSS week
138 : uint16_t gnss_week;
139 : vec3_32_t accel_data_hr; // g * 1e6
140 : vec3_32_t gyro_data_hr; // deg/s * 1e5
141 : struct PACKED {
142 : uint16_t pressure_pa2; // Pascals/2
143 : int32_t baro_alt; // meters*100
144 : } baro_data;
145 : vec3_16_t mag_data; // nT/10
146 : struct PACKED {
147 : uint16_t yaw; // deg*100
148 : int16_t pitch; // deg*100
149 : int16_t roll; // deg*100
150 : } orientation_angles; // 321 euler order?
151 : vec3_32_t velocity; // m/s * 100
152 : struct PACKED {
153 : int32_t lat; // deg*1e7
154 : int32_t lon; // deg*1e7
155 : int32_t alt; // m*100, AMSL
156 : } position;
157 : vec3_u8_t kf_vel_covariance; // mm/s
158 : vec3_u16_t kf_pos_covariance; // mm
159 : uint16_t unit_status; // set ILABS_UNIT_STATUS_*
160 : gnss_extended_info_t gnss_extended_info;
161 : uint8_t num_sats;
162 : struct PACKED {
163 : int32_t lat; // deg*1e7
164 : int32_t lon; // deg*1e7
165 : int32_t alt; // m*100
166 : } gnss_position;
167 : struct PACKED {
168 : int32_t hor_speed; // m/s*100
169 : uint16_t track_over_ground; // deg*100
170 : int32_t ver_speed; // m/s*100
171 : } gnss_vel_track;
172 : uint32_t gnss_pos_timestamp; // ms
173 : gnss_info_short_t gnss_info_short;
174 : uint8_t gnss_new_data;
175 : uint8_t gnss_jam_status;
176 : int32_t differential_pressure; // mbar*1e4
177 : int16_t true_airspeed; // m/s*100
178 : vec3_16_t wind_speed; // m/s*100
179 : uint16_t air_data_status;
180 : uint16_t supply_voltage; // V*100
181 : int16_t temperature; // degC*10
182 : uint16_t unit_status2;
183 : struct PACKED {
184 : uint16_t heading; // deg*100
185 : int16_t pitch; // deg*100
186 : } gnss_angles;
187 : uint8_t gnss_angle_pos_type;
188 : uint32_t gnss_heading_timestamp; // ms
189 : struct PACKED {
190 : uint16_t gdop;
191 : uint16_t pdop;
192 : uint16_t hdop;
193 : uint16_t vdop;
194 : uint16_t tdop;
195 : } gnss_dop; // 10e3
196 : uint8_t ins_sol_status;
197 : };
198 :
199 : AP_ExternalAHRS::gps_data_message_t gps_data;
200 : AP_ExternalAHRS::mag_data_message_t mag_data;
201 : AP_ExternalAHRS::baro_data_message_t baro_data;
202 : AP_ExternalAHRS::ins_data_message_t ins_data;
203 : AP_ExternalAHRS::airspeed_data_message_t airspeed_data;
204 :
205 : uint16_t buffer_ofs;
206 : uint8_t buffer[256]; // max for normal message set is 167+8
207 :
208 : protected:
209 :
210 46 : uint8_t num_gps_sensors(void) const override {
211 46 : return 1;
212 : }
213 :
214 : private:
215 : AP_HAL::UARTDriver *uart;
216 : int8_t port_num;
217 : uint32_t baudrate;
218 : bool setup_complete;
219 :
220 : void update_thread();
221 : bool check_uart();
222 : bool check_header(const ILabsHeader *h) const;
223 :
224 : // re-sync on header bytes
225 : void re_sync(void);
226 :
227 : static const struct MessageLength {
228 : MessageType mtype;
229 : uint8_t length;
230 : } message_lengths[];
231 :
232 : struct {
233 : float baro_alt;
234 : Vector3f kf_vel_covariance;
235 : Vector3f kf_pos_covariance;
236 : uint16_t unit_status;
237 : uint16_t unit_status2;
238 : float differential_pressure;
239 : float true_airspeed;
240 : Vector3f wind_speed;
241 : uint16_t air_data_status;
242 : float supply_voltage;
243 : uint8_t ins_sol_status;
244 : } state2;
245 :
246 : struct {
247 : float lat;
248 : float lng;
249 : float alt;
250 : float hor_speed;
251 : float ver_speed;
252 : float track_over_ground;
253 : uint8_t new_data;
254 : uint32_t pos_timestamp;
255 : uint32_t heading_timestamp;
256 : uint8_t spoof_status;
257 : uint8_t jam_status;
258 : uint8_t angle_pos_type;
259 : gnss_info_short_t info_short;
260 : float heading;
261 : float pitch;
262 : float gdop;
263 : float pdop;
264 : float tdop;
265 : } gnss_data;
266 :
267 : uint16_t last_unit_status;
268 : uint16_t last_unit_status2;
269 : uint16_t last_air_data_status;
270 : uint8_t last_spoof_status;
271 : uint8_t last_jam_status;
272 :
273 : uint32_t last_critical_msg_ms;
274 :
275 : uint32_t last_att_ms;
276 : uint32_t last_vel_ms;
277 : uint32_t last_pos_ms;
278 : uint32_t last_gps_ms;
279 : };
280 :
281 : #endif // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
282 :
|
