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 : //
17 : // Septentrio GPS driver for ArduPilot.
18 : // Code by Michael Oborne
19 : //
20 : #pragma once
21 :
22 : #include "AP_GPS.h"
23 : #include "GPS_Backend.h"
24 :
25 : #if AP_GPS_SBF_ENABLED
26 :
27 : #define SBF_DISK_ACTIVITY (1 << 7)
28 : #define SBF_DISK_FULL (1 << 8)
29 : #define SBF_DISK_MOUNTED (1 << 9)
30 :
31 : class AP_GPS_SBF : public AP_GPS_Backend
32 : {
33 : public:
34 : AP_GPS_SBF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
35 : ~AP_GPS_SBF();
36 :
37 0 : AP_GPS_FixType highest_supported_status(void) override { return AP_GPS_FixType::RTK_FIXED; }
38 :
39 : // Methods
40 : bool read() override;
41 :
42 3 : const char *name() const override { return "SBF"; }
43 :
44 : bool is_configured (void) const override;
45 :
46 : void broadcast_configuration_failure_reason(void) const override;
47 :
48 : #if HAL_GCS_ENABLED
49 65 : bool supports_mavlink_gps_rtk_message(void) const override { return true; };
50 : #endif
51 :
52 : // get the velocity lag, returns true if the driver is confident in the returned value
53 10699 : bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ;
54 :
55 : bool is_healthy(void) const override;
56 :
57 : bool logging_healthy(void) const override;
58 :
59 : bool prepare_for_arming(void) override;
60 :
61 0 : bool get_error_codes(uint32_t &error_codes) const override { error_codes = RxError; return true; };
62 :
63 : private:
64 :
65 : bool parse(uint8_t temp);
66 : bool process_message();
67 :
68 : static const uint8_t SBF_PREAMBLE1 = '$';
69 : static const uint8_t SBF_PREAMBLE2 = '@';
70 :
71 : uint8_t _init_blob_index;
72 : uint32_t _init_blob_time;
73 : enum class Config_State {
74 : Baud_Rate,
75 : SSO,
76 : Blob,
77 : SBAS,
78 : SGA,
79 : Constellation,
80 : Complete
81 : };
82 : Config_State config_step;
83 : char *config_string;
84 : static constexpr const char* _initialisation_blob[] = {
85 : "srd,Moderate,UAV",
86 : "sem,PVT,5",
87 : "spm,Rover,all",
88 : "sso,Stream2,Dsk1,postprocess+event+comment+ReceiverStatus,msec100",
89 : #if defined (GPS_SBF_EXTRA_CONFIG)
90 : GPS_SBF_EXTRA_CONFIG
91 : #endif
92 : };
93 : static constexpr const char* sbas_off = "sst, -SBAS";
94 : static constexpr const char* sbas_on_blob[] = {
95 : "snt,+GEOL1+GEOL5",
96 : "sst,+SBAS",
97 : "ssbc,auto,Operational,MixedSystems,auto",
98 : };
99 : uint32_t _config_last_ack_time;
100 :
101 : const char* _port_enable = "\nSSSSSSSSSS\n";
102 :
103 : uint32_t crc_error_counter = 0;
104 : uint32_t RxState;
105 : uint32_t RxError;
106 :
107 : void mount_disk(void) const;
108 : void unmount_disk(void) const;
109 : bool _has_been_armed;
110 :
111 : enum sbf_ids {
112 : DOP = 4001,
113 : PVTGeodetic = 4007,
114 : ReceiverStatus = 4014,
115 : BaseVectorGeod = 4028,
116 : VelCovGeodetic = 5908,
117 : AttEulerCov = 5939,
118 : AuxAntPositions = 5942,
119 : };
120 :
121 : struct PACKED msg4007 // PVTGeodetic
122 : {
123 : uint32_t TOW;
124 : uint16_t WNc;
125 : uint8_t Mode;
126 : uint8_t Error;
127 : double Latitude;
128 : double Longitude;
129 : double Height;
130 : float Undulation;
131 : float Vn;
132 : float Ve;
133 : float Vu;
134 : float COG;
135 : double RxClkBias;
136 : float RxClkDrift;
137 : uint8_t TimeSystem;
138 : uint8_t Datum;
139 : uint8_t NrSV;
140 : uint8_t WACorrInfo;
141 : uint16_t ReferenceID;
142 : uint16_t MeanCorrAge;
143 : uint32_t SignalInfo;
144 : uint8_t AlertFlag;
145 : // rev1
146 : uint8_t NrBases;
147 : uint16_t PPPInfo;
148 : // rev2
149 : uint16_t Latency;
150 : uint16_t HAccuracy;
151 : uint16_t VAccuracy;
152 : uint8_t Misc;
153 : };
154 :
155 : struct PACKED msg4001 // DOP
156 : {
157 : uint32_t TOW;
158 : uint16_t WNc;
159 : uint8_t NrSV;
160 : uint8_t Reserved;
161 : uint16_t PDOP;
162 : uint16_t TDOP;
163 : uint16_t HDOP;
164 : uint16_t VDOP;
165 : float HPL;
166 : float VPL;
167 : };
168 :
169 : struct PACKED msg4014 // ReceiverStatus (v2)
170 : {
171 : uint32_t TOW;
172 : uint16_t WNc;
173 : uint8_t CPULoad;
174 : uint8_t ExtError;
175 : uint32_t UpTime;
176 : uint32_t RxState;
177 : uint32_t RxError;
178 : // remaining data is AGCData, which we don't have a use for, don't extract the data
179 : };
180 :
181 : struct PACKED VectorInfoGeod {
182 : uint8_t NrSV;
183 : uint8_t Error;
184 : uint8_t Mode;
185 : uint8_t Misc;
186 : double DeltaEast;
187 : double DeltaNorth;
188 : double DeltaUp;
189 : float DeltaVe;
190 : float DeltaVn;
191 : float DeltaVu;
192 : uint16_t Azimuth;
193 : int16_t Elevation;
194 : uint8_t ReferenceID;
195 : uint16_t CorrAge;
196 : uint32_t SignalInfo;
197 : };
198 :
199 : struct PACKED msg4028 // BaseVectorGeod
200 : {
201 : uint32_t TOW;
202 : uint16_t WNc;
203 : uint8_t N; // number of baselines
204 : uint8_t SBLength;
205 : VectorInfoGeod info; // there can be multiple baselines here, but we will only consume the first one, so don't worry about anything after
206 : };
207 :
208 : struct PACKED msg5908 // VelCovGeodetic
209 : {
210 : uint32_t TOW;
211 : uint16_t WNc;
212 : uint8_t Mode;
213 : uint8_t Error;
214 : float Cov_VnVn;
215 : float Cov_VeVe;
216 : float Cov_VuVu;
217 : float Cov_DtDt;
218 : float Cov_VnVe;
219 : float Cov_VnVu;
220 : float Cov_VnDt;
221 : float Cov_VeVu;
222 : float Cov_VeDt;
223 : float Cov_VuDt;
224 : };
225 :
226 : struct PACKED msg5939 // AttEulerCoV
227 : {
228 : uint32_t TOW; // receiver time stamp, 0.001s
229 : uint16_t WNc; // receiver time stamp, 1 week
230 : uint8_t Reserved; // unused
231 : uint8_t Error; // error code. bit 0-1:antenna 1, bit 2-3:antenna2, bit 7: when att not requested
232 : // 00b:no error, 01b:not enough meausurements, 10b:antennas are on one line, 11b:inconsistent with manual anntena pos info
233 : float Cov_HeadHead; // heading estimate variance
234 : float Cov_PitchPitch; // pitch estimate variance
235 : float Cov_RollRoll; // roll estimate variance
236 : float Cov_HeadPitch; // covariance between Euler angle estimates. Always set to Do-No-Use values
237 : float Cov_HeadRoll;
238 : float Cov_PitchRoll;
239 : };
240 :
241 : struct PACKED AuxAntPositionSubBlock {
242 : uint8_t NrSV; // total number of satellites tracked by the antenna
243 : uint8_t Error; // aux antenna position error code
244 : uint8_t AmbiguityType; // aux antenna positions obtained with 0: fixed ambiguities, 1: float ambiguities
245 : uint8_t AuxAntID; // aux antenna ID: 1 for the first auxiliary antenna, 2 for the second, etc.
246 : double DeltaEast; // position in East direction (relative to main antenna)
247 : double DeltaNorth; // position in North direction (relative to main antenna)
248 : double DeltaUp; // position in Up direction (relative to main antenna)
249 : double EastVel; // velocity in East direction (relative to main antenna)
250 : double NorthVel; // velocity in North direction (relative to main antenna)
251 : double UpVel; // velocity in Up direction (relative to main antenna)
252 : };
253 :
254 : struct PACKED msg5942 // AuxAntPositions
255 : {
256 : uint32_t TOW;
257 : uint16_t WNc;
258 : uint8_t N; // number of AuxAntPosition sub-blocks in this AuxAntPositions block
259 : uint8_t SBLength; // length of one sub-block in bytes
260 : AuxAntPositionSubBlock ant1; // first aux antennas position
261 : };
262 :
263 : union PACKED msgbuffer {
264 : msg4007 msg4007u;
265 : msg4001 msg4001u;
266 : msg4014 msg4014u;
267 : msg4028 msg4028u;
268 : msg5908 msg5908u;
269 : msg5939 msg5939u;
270 : msg5942 msg5942u;
271 : uint8_t bytes[256];
272 : };
273 :
274 : struct sbf_msg_parser_t
275 : {
276 : enum
277 : {
278 : PREAMBLE1 = 0,
279 : PREAMBLE2,
280 : CRC1,
281 : CRC2,
282 : BLOCKID1,
283 : BLOCKID2,
284 : LENGTH1,
285 : LENGTH2,
286 : DATA,
287 : COMMAND_LINE // used to parse command responses
288 : } sbf_state;
289 : uint16_t preamble;
290 : uint16_t crc;
291 : uint16_t blockid;
292 : uint16_t length;
293 : msgbuffer data;
294 : uint16_t read;
295 : } sbf_msg;
296 :
297 : enum {
298 : SOFTWARE = (1 << 3), // set upon detection of a software warning or error. This bit is reset by the command lif, error
299 : WATCHDOG = (1 << 4), // set when the watch-dog expired at least once since the last power-on.
300 : CONGESTION = (1 << 6), // set when an output data congestion has been detected on at least one of the communication ports of the receiver during the last second.
301 : MISSEDEVENT = (1 << 8), // set when an external event congestion has been detected during the last second. It indicates that the receiver is receiving too many events on its EVENTx pins.
302 : CPUOVERLOAD = (1 << 9), // set when the CPU load is larger than 90%. If this bit is set, receiver operation may be unreliable and the user must decrease the processing load by following the recommendations in the User Manual.
303 : INVALIDCONFIG = (1 << 10), // set if one or more configuration file (permission or channel configuration) is invalid or absent.
304 : OUTOFGEOFENCE = (1 << 11), // set if the receiver is currently out of its permitted region of operation (geo-fencing).
305 : };
306 :
307 : static constexpr const char *portIdentifiers[] = { "COM", "USB", "IP1", "NTR", "IPS", "IPR" };
308 : char portIdentifier[5];
309 : uint8_t portLength;
310 : bool readyForCommand;
311 : };
312 : #endif
|
