forked from ArduPilot/ardupilot
/
AP_ExternalAHRS_InertialLabs.h
232 lines (202 loc) · 6.55 KB
/
AP_ExternalAHRS_InertialLabs.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
support for serial connected InertialLabs INS system
*/
#pragma once
#include "AP_ExternalAHRS_config.h"
#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED
#include "AP_ExternalAHRS_backend.h"
class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
public:
AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
// get serial port number, -1 for not enabled
int8_t get_port(void) const override;
// accessors for AP_AHRS
bool healthy(void) const override;
bool initialised(void) const override;
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
void get_filter_status(nav_filter_status &status) const override;
void send_status_report(class GCS_MAVLINK &link) const override;
// check for new data
void update() override {
check_uart();
}
// Get model/type name
const char* get_name() const override {
return "ILabs";
}
enum class MessageType : uint8_t {
GPS_INS_TIME_MS = 0x01,
GPS_WEEK = 0x3C,
ACCEL_DATA_HR = 0x23,
GYRO_DATA_HR = 0x21,
BARO_DATA = 0x25,
MAG_DATA = 0x24,
ORIENTATION_ANGLES = 0x07,
VELOCITIES = 0x12,
POSITION = 0x10,
KF_VEL_COVARIANCE = 0x58,
KF_POS_COVARIANCE = 0x57,
UNIT_STATUS = 0x53,
GNSS_EXTENDED_INFO = 0x4A,
NUM_SATS = 0x3B,
GNSS_POSITION = 0x30,
GNSS_VEL_TRACK = 0x32,
GNSS_POS_TIMESTAMP = 0x3E,
GNSS_INFO_SHORT = 0x36,
GNSS_NEW_DATA = 0x41,
GNSS_JAM_STATUS = 0xC0,
DIFFERENTIAL_PRESSURE = 0x28,
TRUE_AIRSPEED = 0x86,
WIND_SPEED = 0x8A,
AIR_DATA_STATUS = 0x8D,
SUPPLY_VOLTAGE = 0x50,
TEMPERATURE = 0x52,
UNIT_STATUS2 = 0x5A,
};
/*
packets consist of:
ILabsHeader
list of MessageType
sequence of ILabsData
checksum
*/
struct PACKED ILabsHeader {
uint16_t magic; // 0x55AA
uint8_t msg_type; // always 1 for INS data
uint8_t msg_id; // always 0x95
uint16_t msg_len; // msg_len+2 is total packet length
};
struct PACKED vec3_16_t {
int16_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct PACKED vec3_32_t {
int32_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct PACKED vec3_u8_t {
uint8_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct PACKED vec3_u16_t {
uint16_t x,y,z;
Vector3f tofloat(void) {
return Vector3f(x,y,z);
}
};
struct gnss_extended_info_t {
uint8_t fix_type;
uint8_t spoofing_status;
};
struct gnss_info_short_t {
uint8_t info1;
uint8_t info2;
};
union PACKED ILabsData {
uint32_t gps_time_ms; // ms since start of GPS week
uint16_t gps_week;
vec3_32_t accel_data_hr; // g * 1e6
vec3_32_t gyro_data_hr; // deg/s * 1e5
struct PACKED {
uint16_t pressure_pa2; // Pascals/2
int32_t baro_alt; // meters*100
} baro_data;
vec3_16_t mag_data; // nT/10
struct PACKED {
int16_t yaw; // deg*100
int16_t pitch; // deg*100
int16_t roll; // deg*100
} orientation_angles; // 321 euler order?
vec3_32_t velocity; // m/s * 100
struct PACKED {
int32_t lat; // deg*1e7
int32_t lon; // deg*1e7
int32_t alt; // m*100, AMSL
} position;
vec3_u8_t kf_vel_covariance; // mm/s
vec3_u16_t kf_pos_covariance;
uint16_t unit_status; // set ILABS_UNIT_STATUS_*
gnss_extended_info_t gnss_extended_info;
uint8_t num_sats;
struct PACKED {
int32_t hor_speed; // m/s*100
uint16_t track_over_ground; // deg*100
int32_t ver_speed; // m/s*100
} gnss_vel_track;
uint32_t gnss_pos_timestamp; // ms
gnss_info_short_t gnss_info_short;
uint8_t gnss_new_data;
uint8_t gnss_jam_status;
int32_t differential_pressure; // mbar*1e4
int16_t true_airspeed; // m/s*100
vec3_16_t wind_speed; // m/s*100
uint16_t air_data_status;
uint16_t supply_voltage; // V*100
int16_t temperature; // degC*10
uint16_t unit_status2;
};
AP_ExternalAHRS::gps_data_message_t gps_data;
AP_ExternalAHRS::mag_data_message_t mag_data;
AP_ExternalAHRS::baro_data_message_t baro_data;
AP_ExternalAHRS::ins_data_message_t ins_data;
AP_ExternalAHRS::airspeed_data_message_t airspeed_data;
uint16_t buffer_ofs;
uint8_t buffer[256]; // max for normal message set is 167+8
protected:
uint8_t num_gps_sensors(void) const override {
return 1;
}
private:
AP_HAL::UARTDriver *uart;
int8_t port_num;
uint32_t baudrate;
bool setup_complete;
void update_thread();
bool check_uart();
bool check_header(const ILabsHeader *h) const;
// re-sync on header bytes
void re_sync(void);
static const struct MessageLength {
MessageType mtype;
uint8_t length;
} message_lengths[];
struct {
Vector3f kf_vel_covariance;
Vector3f kf_pos_covariance;
uint32_t gnss_ins_time_ms;
uint16_t unit_status;
uint16_t unit_status2;
gnss_extended_info_t gnss_extended_info;
gnss_info_short_t gnss_info_short;
uint8_t gnss_new_data;
uint8_t gnss_jam_status;
float differential_pressure;
float true_airspeed;
Vector3f wind_speed;
uint16_t air_data_status;
float supply_voltage;
} state2;
uint32_t last_att_ms;
uint32_t last_vel_ms;
uint32_t last_pos_ms;
uint32_t last_gps_ms;
};
#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED