CoCalc -- AP_GPS

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_GPS/AP_GPS_NOVA.cpp
Views: ¹⁷⁹⁸
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
//  Novatel/Tersus/ComNav GPS driver for ArduPilot.
17
//  Code by Michael Oborne
18
//  Derived from http://www.novatel.com/assets/Documents/Manuals/om-20000129.pdf
19

20
#include "AP_GPS.h"
21
#include "AP_GPS_NOVA.h"
22
#include <AP_Logger/AP_Logger.h>
23

24
#if AP_GPS_NOVA_ENABLED
25

26
extern const AP_HAL::HAL& hal;
27

28
#define NOVA_DEBUGGING 0
29

30
#if NOVA_DEBUGGING
31
#include <cstdio>
32
 # define Debug(fmt, args ...)                  \
33
do {                                            \
34
    printf("%s:%d: " fmt "\n",     \
35
                        __FUNCTION__, __LINE__, \
36
                        ## args);               \
37
    hal.scheduler->delay(1);                    \
38
} while(0)
39
#else
40
 # define Debug(fmt, args ...)
41
#endif
42

43
AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps,
44
                         AP_GPS::Params &_params,
45
                         AP_GPS::GPS_State &_state,
46
                         AP_HAL::UARTDriver *_port) :
47
    AP_GPS_Backend(_gps, _params, _state, _port)
48
{
49
    nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
50

51
    nova_msg.header.data[0] = NOVA_PREAMBLE1;
52
    nova_msg.header.data[1] = NOVA_PREAMBLE2;
53
    nova_msg.header.data[2] = NOVA_PREAMBLE3;
54

55
    if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
56
        const char *init_str = _initialisation_blob[0];
57
        const char *init_str1 = _initialisation_blob[1];
58

59
        port->write((const uint8_t*)init_str, strlen(init_str));
60
        port->write((const uint8_t*)init_str1, strlen(init_str1));
61
    }
62
}
63

64
const char* const AP_GPS_NOVA::_initialisation_blob[4] {
65
    "\r\n\r\nunlogall\r\n", // cleanup enviroment
66
    "log bestposb ontime 0.2 0 nohold\r\n",
67
    "log bestvelb ontime 0.2 0 nohold\r\n",
68
    "log psrdopb ontime 0.2 0 nohold\r\n",
69
};
70

71
// Process all bytes available from the stream
72
//
73
bool
74
AP_GPS_NOVA::read(void)
75
{
76
    if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
77
        const uint32_t now = AP_HAL::millis();
78

79
        if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) {
80
            const char *init_str = _initialisation_blob[_init_blob_index];
81

82
            if (now > _init_blob_time) {
83
                port->write((const uint8_t*)init_str, strlen(init_str));
84
                _init_blob_time = now + 200;
85
                _init_blob_index++;
86
            }
87
        }
88
    }
89

90
    bool ret = false;
91
    for (uint16_t i=0; i<8192; i++) {
92
        uint8_t temp;
93
        if (!port->read(temp)) {
94
            break;
95
        }
96
#if AP_GPS_DEBUG_LOGGING_ENABLED
97
        log_data(&temp, 1);
98
#endif
99
        ret |= parse(temp);
100
    }
101
    
102
    return ret;
103
}
104

105
bool
106
AP_GPS_NOVA::parse(uint8_t temp)
107
{
108
    switch (nova_msg.nova_state)
109
    {
110
        default:
111
        case nova_msg_parser::PREAMBLE1:
112
            if (temp == NOVA_PREAMBLE1) {
113
                nova_msg.nova_state = nova_msg_parser::PREAMBLE2;
114
            }
115
            nova_msg.read = 0;
116
            break;
117
        case nova_msg_parser::PREAMBLE2:
118
            if (temp == NOVA_PREAMBLE2) {
119
                nova_msg.nova_state = nova_msg_parser::PREAMBLE3;
120
            } else {
121
                nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
122
            }
123
            break;
124
        case nova_msg_parser::PREAMBLE3:
125
            if (temp == NOVA_PREAMBLE3) {
126
                nova_msg.nova_state = nova_msg_parser::HEADERLENGTH;
127
            } else {
128
                nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
129
            }
130
            break;
131
        case nova_msg_parser::HEADERLENGTH:
132
            Debug("NOVA HEADERLENGTH\n");
133
            nova_msg.header.data[3] = temp;
134
            nova_msg.header.nova_headeru.headerlength = temp;
135
            nova_msg.nova_state = nova_msg_parser::HEADERDATA;
136
            nova_msg.read = 4;
137
            break;
138
        case nova_msg_parser::HEADERDATA:
139
            if (nova_msg.read >= sizeof(nova_msg.header.data)) {
140
                Debug("parse header overflow length=%u\n", (unsigned)nova_msg.read);
141
                nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
142
                break;
143
            }
144
            nova_msg.header.data[nova_msg.read] = temp;
145
            nova_msg.read++;
146
            if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength) {
147
                nova_msg.nova_state = nova_msg_parser::DATA;
148
            }
149
            break;
150
        case nova_msg_parser::DATA:
151
            if (nova_msg.read >= sizeof(nova_msg.data)) {
152
                Debug("parse data overflow length=%u msglength=%u\n", (unsigned)nova_msg.read,nova_msg.header.nova_headeru.messagelength);
153
                nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
154
                break;
155
            }
156
            nova_msg.data.bytes[nova_msg.read - nova_msg.header.nova_headeru.headerlength] = temp;
157
            nova_msg.read++;
158
            if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength)) {
159
                Debug("NOVA DATA exit\n");
160
                nova_msg.nova_state = nova_msg_parser::CRC1;
161
            }
162
            break;
163
        case nova_msg_parser::CRC1:
164
            nova_msg.crc = (uint32_t) (temp << 0);
165
            nova_msg.nova_state = nova_msg_parser::CRC2;
166
            break;
167
        case nova_msg_parser::CRC2:
168
            nova_msg.crc += (uint32_t) (temp << 8);
169
            nova_msg.nova_state = nova_msg_parser::CRC3;
170
            break;
171
        case nova_msg_parser::CRC3:
172
            nova_msg.crc += (uint32_t) (temp << 16);
173
            nova_msg.nova_state = nova_msg_parser::CRC4;
174
            break;
175
        case nova_msg_parser::CRC4:
176
            nova_msg.crc += (uint32_t) (temp << 24);
177
            nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
178

179
            uint32_t crc = crc_crc32((uint32_t)0, (uint8_t *)&nova_msg.header.data, (uint32_t)nova_msg.header.nova_headeru.headerlength);
180
            crc = crc_crc32(crc, (uint8_t *)&nova_msg.data, (uint32_t)nova_msg.header.nova_headeru.messagelength);
181

182
            if (nova_msg.crc == crc) {
183
                return process_message();
184
            } else {
185
                Debug("crc failed");
186
                crc_error_counter++;
187
            }
188
            break;
189
    }
190

191
    return false;
192
}
193

194
bool
195
AP_GPS_NOVA::process_message(void)
196
{
197
    const uint16_t messageid = nova_msg.header.nova_headeru.messageid;
198

199
    Debug("NOVA process_message messid=%u\n",messageid);
200

201
    check_new_itow(nova_msg.header.nova_headeru.tow, nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength);
202
    
203
    if (messageid == 42) // bestpos
204
    {
205
        const bestpos &bestposu = nova_msg.data.bestposu;
206

207
        state.time_week = nova_msg.header.nova_headeru.week;
208
        state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow;
209
        state.last_gps_time_ms = AP_HAL::millis();
210

211
        state.location.lat = (int32_t) (bestposu.lat * (double)1e7);
212
        state.location.lng = (int32_t) (bestposu.lng * (double)1e7);
213
        state.have_undulation = true;
214
        state.undulation = bestposu.undulation;
215
        set_alt_amsl_cm(state, bestposu.hgt * 100);
216

217
        state.num_sats = bestposu.svsused;
218

219
        state.horizontal_accuracy =  norm(bestposu.latsdev, bestposu.lngsdev);
220
        state.vertical_accuracy = (float) bestposu.hgtsdev;
221
        state.have_horizontal_accuracy = true;
222
        state.have_vertical_accuracy = true;
223
        state.rtk_age_ms = bestposu.diffage * 1000;
224
        state.rtk_num_sats = bestposu.svsused;
225

226
        if (bestposu.solstat == 0) // have a solution
227
        {
228
            switch (bestposu.postype)
229
            {
230
                case 16:
231
                    state.status = AP_GPS::GPS_OK_FIX_3D;
232
                    break;
233
                case 17: // psrdiff
234
                case 18: // waas
235
                case 20: // omnistar
236
                case 68: // ppp_converg
237
                case 69: // ppp
238
                    state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
239
                    break;
240
                case 32: // l1 float
241
                case 33: // iono float
242
                case 34: // narrow float
243
                    state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
244
                    break;
245
                case 48: // l1 int
246
                case 50: // narrow int
247
                    state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
248
                    break;
249
                case 0: // NONE
250
                case 1: // FIXEDPOS
251
                case 2: // FIXEDHEIGHT
252
                default:
253
                    state.status = AP_GPS::NO_FIX;
254
                    break;
255
            }
256
        } else {
257
            state.status = AP_GPS::NO_FIX;
258
        }
259
        
260
        _new_position = true;
261
    }
262

263
    if (messageid == 99) // bestvel
264
    {
265
        const bestvel &bestvelu = nova_msg.data.bestvelu;
266

267
        state.ground_speed = (float) bestvelu.horspd;
268
        state.ground_course = (float) bestvelu.trkgnd;
269
        fill_3d_velocity();
270
        state.velocity.z = -(float) bestvelu.vertspd;
271
        state.have_vertical_velocity = true;
272
        
273
        _last_vel_time = (uint32_t) nova_msg.header.nova_headeru.tow;
274
        _new_speed = true;
275
    }
276

277
    if (messageid == 174) // psrdop
278
    {
279
        const psrdop &psrdopu = nova_msg.data.psrdopu;
280

281
        state.hdop = (uint16_t) (psrdopu.hdop*100);
282
        state.vdop = (uint16_t) (psrdopu.htdop*100);
283
        return false;
284
    }
285

286
    // ensure out position and velocity stay insync
287
    if (_new_position && _new_speed && _last_vel_time == state.time_week_ms) {
288
        _new_speed = _new_position = false;
289
        
290
        return true;
291
    }
292
    
293
    return false;
294
}
295

296
#endif
297

298
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
