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 : // Trimble GPS driver for ArduPilot.
18 : // Code by Michael Oborne
19 : //
20 : // Usage in SITL with hardware for debugging:
21 : // sim_vehicle.py -v Plane -A "--serial3=uart:/dev/ttyUSB0" --console --map -DG
22 : // param set GPS1_TYPE 11 // GSOF
23 : // param set SERIAL3_PROTOCOL 5 // GPS
24 : //
25 : // Usage with NET parameters and ethernet in SITL with hardware:
26 : // param set NET_ENABLE 1
27 : // param set NET_P1_TYPE 2
28 : // param set NET_P1_PROTOCOL 5
29 : // param set SERIAL3_PROTOCOL 0
30 : // param set SIM_GPS1_TYPE 0
31 : // param set NET_P1_PORT 44444
32 : // param set GPS1_TYPE 11
33 : // param set GPS_AUTO_CONFIG 0
34 : //
35 : // Pure SITL usage:
36 : // sim_vehicle.py -v Plane --console --map -DG
37 : // param set SIM_GPS1_TYPE 11 // GSOF
38 : // param set GPS1_TYPE 11 // GSOF
39 : // param set SERIAL3_PROTOCOL 5 // GPS
40 :
41 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
42 :
43 : #include "AP_GPS.h"
44 : #include "AP_GPS_GSOF.h"
45 : #include <AP_Logger/AP_Logger.h>
46 : #include <AP_HAL/utility/sparse-endian.h>
47 : #include <GCS_MAVLink/GCS.h>
48 :
49 : #if AP_GPS_GSOF_ENABLED
50 :
51 : extern const AP_HAL::HAL& hal;
52 :
53 1 : AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps,
54 : AP_GPS::Params &_params,
55 : AP_GPS::GPS_State &_state,
56 1 : AP_HAL::UARTDriver *_port) :
57 1 : AP_GPS_Backend(_gps, _params, _state, _port)
58 : {
59 :
60 1 : if (port == nullptr) {
61 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has no port", state.instance);
62 0 : return;
63 : }
64 :
65 1 : const uint16_t gsofmsgreq[5] = {
66 : AP_GSOF::POS_TIME,
67 : AP_GSOF::POS,
68 : AP_GSOF::VEL,
69 : AP_GSOF::DOP,
70 : AP_GSOF::POS_SIGMA
71 : };
72 : // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0
73 : // The maximum number of outputs allowed with GSOF is 10
74 : static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10.");
75 1 : requested_msgs = AP_GSOF::MsgTypes(gsofmsgreq);
76 :
77 :
78 1 : constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
79 1 : params.com_port.set_default(default_com_port);
80 1 : const auto com_port = params.com_port;
81 1 : if (!validate_com_port(com_port)) {
82 : // The user parameter for COM port is not a valid GSOF port
83 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, (unsigned)com_port);
84 0 : return;
85 : }
86 :
87 1 : if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
88 1 : requestBaud(static_cast<HW_Port>(unsigned(com_port)));
89 : }
90 :
91 1 : const uint32_t now = AP_HAL::millis();
92 1 : gsofmsg_time = now + 110;
93 : }
94 :
95 693 : bool AP_GPS_GSOF::configure(void)
96 : {
97 693 : const uint32_t now = AP_HAL::millis();
98 :
99 693 : if (gsofmsgreq_index < (requested_msgs.count())) {
100 30 : const auto com_port = params.com_port.get();
101 30 : if (!validate_com_port(com_port)) {
102 : // The user parameter for COM port is not a valid GSOF port
103 0 : return false;
104 : }
105 30 : if (now > gsofmsg_time) {
106 13 : for (uint16_t i = next_req_gsof; i < requested_msgs.size(); i++){
107 13 : if (requested_msgs.get(i)) {
108 5 : next_req_gsof = i;
109 5 : break;
110 : }
111 : }
112 5 : requestGSOF(next_req_gsof, static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
113 5 : gsofmsg_time = now + 110;
114 5 : gsofmsgreq_index++;
115 5 : next_req_gsof++;
116 : }
117 : }
118 693 : return true;
119 : }
120 :
121 : // Process all bytes available from the stream
122 : //
123 : bool
124 693 : AP_GPS_GSOF::read(void)
125 : {
126 693 : if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
127 693 : if (!configure()) return false;
128 : }
129 8295 : while (port->available() > 0) {
130 7661 : const uint8_t temp = port->read();
131 : #if AP_GPS_DEBUG_LOGGING_ENABLED
132 : log_data(&temp, 1);
133 : #endif
134 7661 : AP_GSOF::MsgTypes parsed {};
135 7661 : const int parse_status = parse(temp, parsed);
136 7661 : if(parse_status == PARSED_GSOF_DATA) {
137 124 : if (parsed.get(AP_GSOF::POS_TIME) &&
138 123 : parsed.get(AP_GSOF::POS) &&
139 121 : parsed.get(AP_GSOF::VEL) &&
140 184 : parsed.get(AP_GSOF::DOP) &&
141 60 : parsed.get(AP_GSOF::POS_SIGMA)
142 : )
143 : {
144 59 : pack_state_data();
145 59 : return true;
146 : }
147 : }
148 : }
149 :
150 634 : return false;
151 : }
152 :
153 : void
154 1 : AP_GPS_GSOF::requestBaud(const HW_Port portindex)
155 : {
156 1 : uint8_t buffer[19] = {0x02,0x00,0x64,0x0d,0x00,0x00,0x00, // application file record
157 : 0x03, 0x00, 0x01, 0x00, // file control information block
158 : 0x02, 0x04, static_cast<uint8_t>(portindex), 0x07, 0x00,0x00, // serial port baud format
159 : 0x00,0x03
160 1 : }; // checksum
161 :
162 1 : buffer[4] = packetcount++;
163 :
164 1 : uint8_t checksum = 0;
165 18 : for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) {
166 17 : checksum += buffer[a];
167 : }
168 :
169 1 : buffer[17] = checksum;
170 :
171 1 : port->write((const uint8_t*)buffer, sizeof(buffer));
172 1 : }
173 :
174 : void
175 5 : AP_GPS_GSOF::requestGSOF(const uint8_t messageType, const HW_Port portIndex, const Output_Rate rateHz)
176 : {
177 5 : uint8_t buffer[21] = {0x02,0x00,0x64,0x0f,0x00,0x00,0x00, // application file record
178 : 0x03,0x00,0x01,0x00, // file control information block
179 : 0x07,0x06,0x0a,static_cast<uint8_t>(portIndex),static_cast<uint8_t>(rateHz),0x00,messageType,0x00, // output message record
180 : 0x00,0x03
181 5 : }; // checksum
182 :
183 5 : buffer[4] = packetcount++;
184 :
185 5 : uint8_t checksum = 0;
186 100 : for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) {
187 95 : checksum += buffer[a];
188 : }
189 :
190 5 : buffer[19] = checksum;
191 :
192 5 : port->write((const uint8_t*)buffer, sizeof(buffer));
193 5 : }
194 :
195 : bool
196 31 : AP_GPS_GSOF::validate_com_port(const uint8_t com_port) const
197 : {
198 31 : switch(com_port) {
199 31 : case static_cast<uint8_t>(HW_Port::COM1):
200 : case static_cast<uint8_t>(HW_Port::COM2):
201 31 : return true;
202 0 : default:
203 0 : return false;
204 : }
205 : }
206 :
207 : void
208 59 : AP_GPS_GSOF::pack_state_data()
209 : {
210 : // TODO should we pack time data if there is no fix?
211 59 : state.time_week_ms = pos_time.time_week_ms;
212 59 : state.time_week = pos_time.time_week;
213 59 : state.num_sats = pos_time.num_sats;
214 :
215 59 : if ((pos_time.pos_flags1 & 1)) { // New position
216 59 : state.status = AP_GPS_FixType::FIX_3D;
217 59 : if ((pos_time.pos_flags2 & 1)) { // Differential position
218 59 : state.status = AP_GPS_FixType::DGPS;
219 59 : if (pos_time.pos_flags2 & 2) { // Differential position method
220 59 : if (pos_time.pos_flags2 & 4) {// Differential position method
221 59 : state.status = AP_GPS_FixType::RTK_FIXED;
222 : } else {
223 0 : state.status = AP_GPS_FixType::RTK_FLOAT;
224 : }
225 : }
226 : }
227 : } else {
228 0 : state.status = AP_GPS_FixType::NONE;
229 : }
230 :
231 59 : state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * position.latitude_rad * (double)1e7);
232 59 : state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * position.longitude_rad * (double)1e7);
233 59 : state.location.alt = (int32_t)(position.altitude * 100);
234 59 : state.last_gps_time_ms = AP_HAL::millis();
235 :
236 59 : if ((vel.velocity_flags & 1) == 1) {
237 59 : state.ground_speed = vel.horizontal_velocity;
238 59 : state.ground_course = wrap_360(degrees(vel.heading));
239 59 : fill_3d_velocity();
240 59 : state.velocity.z = -vel.vertical_velocity;
241 59 : state.have_vertical_velocity = true;
242 : }
243 :
244 59 : state.hdop = (uint16_t)(dop.hdop * 100);
245 59 : state.vdop = (uint16_t)(dop.vdop * 100);
246 :
247 59 : state.horizontal_accuracy = (pos_sigma.sigma_east + pos_sigma.sigma_north) / 2;
248 59 : state.vertical_accuracy = pos_sigma.sigma_up;
249 59 : state.have_horizontal_accuracy = true;
250 59 : state.have_vertical_accuracy = true;
251 59 : }
252 : #endif
|
