Line data Source code
1 : /*
2 : Copyright (C) 2025 Kraus Hamdani Aerospace Inc. All rights reserved.
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 : /*
18 : support for serial connected SBG INS system
19 : */
20 :
21 : #pragma once
22 :
23 : #include "AP_ExternalAHRS_config.h"
24 :
25 : #if AP_EXTERNAL_AHRS_SBG_ENABLED
26 :
27 : #include "AP_ExternalAHRS_backend.h"
28 : #include "AP_ExternalAHRS_SBG_structs.h"
29 :
30 : class AP_ExternalAHRS_SBG : public AP_ExternalAHRS_backend {
31 :
32 : public:
33 : AP_ExternalAHRS_SBG(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
34 :
35 : // get serial port number, -1 for not enabled
36 0 : int8_t get_port(void) const override { return (uart == nullptr) ? -1 : port_num; }
37 :
38 : // accessors for AP_AHRS
39 0 : bool healthy(void) const override { return last_received_ms > 0 && (AP_HAL::millis() - last_received_ms < 500); }
40 0 : bool initialised(void) const override { return setup_complete; };
41 : bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
42 :
43 : void get_filter_status(nav_filter_status &status) const override;
44 :
45 : bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
46 :
47 : // check for new data
48 0 : void update() override { }
49 :
50 : // Get model/type name
51 0 : const char* get_name() const override { return "SBG"; }
52 :
53 : protected:
54 :
55 0 : uint8_t num_gps_sensors(void) const override {
56 0 : return 1;
57 : }
58 :
59 : private:
60 :
61 : static constexpr uint8_t SBG_PACKET_SYNC1 = 0xFF;
62 : static constexpr uint8_t SBG_PACKET_SYNC2 = 0x5A;
63 : static constexpr uint8_t SBG_PACKET_ETX = 0x33;
64 : static constexpr uint16_t SBG_PACKET_PAYLOAD_SIZE_MAX = 100; // real sbgCom limit is 4086 but the largest packet we parse is SbgEComLogEkfNav=72 bytes
65 : static constexpr uint16_t SBG_PACKET_OVERHEAD = 9; // sync1, sync2, id, class, lenLSB, lenMSB, crcLSB, crcMSB, etx
66 :
67 : struct Cached {
68 : struct {
69 : AP_ExternalAHRS::gps_data_message_t gps_data;
70 : AP_ExternalAHRS::mag_data_message_t mag_data;
71 : AP_ExternalAHRS::baro_data_message_t baro_data;
72 : AP_ExternalAHRS::ins_data_message_t ins_data;
73 : AP_ExternalAHRS::airspeed_data_message_t airspeed_data;
74 :
75 : float baro_height;
76 :
77 : uint32_t gps_ms;
78 : uint32_t mag_ms;
79 : uint32_t baro_ms;
80 : uint32_t ins_ms;
81 : uint32_t airspeed_ms;
82 : } sensors;
83 :
84 : struct {
85 : SbgEComLogUtc utc;
86 : SbgEComLogGnssVel gnssVel;
87 : SbgEComLogGnssPos gnssPos;
88 : SbgEComLogImuLegacy imuLegacy;
89 : SbgEComLogImuFastLegacy imuFastLegacy;
90 : SbgEComLogImuShort imuShort;
91 : SbgEComLogEkfEuler ekfEuler;
92 : SbgEComLogEkfQuat ekfQuat;
93 : SbgEComLogEkfNav ekfNav; // biggest msg we care about, 72 bytes
94 : SbgEComLogAirData airData;
95 : SbgEComLogMag mag;
96 : SbgEComDeviceInfo deviceInfo;
97 : } sbg;
98 : } cached;
99 :
100 : struct PACKED sbgMessage {
101 :
102 : uint8_t msgid = 0;
103 : uint8_t msgclass = 0;
104 : uint16_t len = 0;
105 : uint8_t data[SBG_PACKET_PAYLOAD_SIZE_MAX];
106 :
107 0 : sbgMessage() {};
108 :
109 0 : sbgMessage(const uint8_t msgClass_, const uint8_t msgId_) {
110 0 : msgid = msgId_;
111 0 : msgclass = msgClass_;
112 0 : };
113 :
114 0 : sbgMessage(const uint8_t msgClass_, const uint8_t msgId_, const uint8_t* payload, const uint16_t payload_len) {
115 0 : msgid = msgId_;
116 0 : msgclass = msgClass_;
117 :
118 0 : if (payload_len > 0 && payload_len <= sizeof(data)) {
119 0 : memcpy(&data, payload, payload_len);
120 0 : len = payload_len;
121 : }
122 0 : };
123 : };
124 :
125 : enum class SBG_PACKET_PARSE_STATE : uint8_t {
126 : SYNC1,
127 : SYNC2,
128 : MSG,
129 : CLASS,
130 : LEN1,
131 : LEN2,
132 : DATA,
133 : CRC1,
134 : CRC2,
135 : ETX,
136 : DROP_THIS_PACKET
137 : };
138 :
139 :
140 : struct SBG_PACKET_INBOUND_STATE {
141 : SBG_PACKET_PARSE_STATE parser;
142 : uint16_t data_count;
143 : uint16_t crc;
144 : sbgMessage msg;
145 : uint16_t data_count_skip; // if we are parsing for a packet larger than we can accept, just stop parsing and wait for this many bytes to pass on by
146 : } _inbound_state;
147 :
148 : void handle_msg(const sbgMessage &msg);
149 : static bool parse_byte(const uint8_t data, sbgMessage &msg, SBG_PACKET_INBOUND_STATE &inbound_state);
150 : void update_thread();
151 : bool check_uart();
152 : static uint16_t calcCRC(const void *pBuffer, uint16_t bufferSize);
153 : static bool send_sbgMessage(AP_HAL::UARTDriver *_uart, const sbgMessage &msg);
154 : static void safe_copy_msg_to_object(uint8_t* dest, const uint16_t dest_len, const uint8_t* src, const uint16_t src_len);
155 : static uint16_t make_gps_week(const SbgEComLogUtc *utc_data);
156 :
157 : bool ekf_is_full_nav;
158 : static bool SbgEkfStatus_is_fullNav(const uint32_t ekfStatus);
159 :
160 : static AP_GPS_FixType SbgGpsPosStatus_to_GpsFixType(const uint32_t gpsPosStatus);
161 :
162 : uint32_t send_MagData_ms;
163 : uint32_t send_AirData_ms;
164 : uint32_t send_mag_error_last_ms;
165 : uint32_t send_air_error_last_ms;
166 : static bool send_MagData(AP_HAL::UARTDriver *_uart);
167 : static bool send_AirData(AP_HAL::UARTDriver *_uart);
168 :
169 : AP_HAL::UARTDriver *uart;
170 : int8_t port_num;
171 : uint32_t baudrate;
172 : bool setup_complete;
173 : uint32_t version_check_ms;
174 : uint32_t last_received_ms;
175 : };
176 :
177 : #endif // AP_EXTERNAL_AHRS_SBG_ENABLED
178 :
|
