Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2189 - 1
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
2
 
3
#include "GPS.h"
4
/*
5
#if defined(ARDUINO) && ARDUINO >= 100
6
        #include "Arduino.h"
7
#else
8
        #include "WProgram.h"
9
#endif
10
*/
11
 
12
void
13
GPS::update(void)
14
{
15
    bool        result;
16
 
17
    // call the GPS driver to process incoming data
18
    result = read();
19
 
20
    // if we did not get a message, and the idle timer has expired, re-init
21
    if (!result) {
22
        if ((millis() - _idleTimer) > idleTimeout) {
23
            _status = NO_GPS;
24
 
25
            init();
26
            // reset the idle timer
27
            _idleTimer = millis();
28
        }
29
    } else {
30
        // we got a message, update our status correspondingly
31
        _status = fix ? GPS_OK : NO_FIX;
32
 
33
        valid_read = true;
34
        new_data = true;
35
 
36
        // reset the idle timer
37
        _idleTimer = millis();
38
 
39
                if (_status == GPS_OK) {
40
                        // update our acceleration
41
                        float deltat = 1.0e-3 * (_idleTimer - last_fix_time);
42
                        float deltav = 1.0e-2 * ((float)ground_speed - (float)_last_ground_speed);
43
                        last_fix_time = _idleTimer;
44
                        _last_ground_speed = ground_speed;
45
 
46
                        if (deltat > 2.0 || deltat == 0) {
47
                                // we didn't get a fix for 2 seconds - set
48
                                // acceleration to zero, as the estimate will be too
49
                                // far out
50
                                _acceleration = 0;
51
                        } else {
52
                                // calculate a mildly smoothed acceleration value
53
                                _acceleration = (0.7 * _acceleration) + (0.3 * (deltav/deltat));
54
                        }
55
                }
56
    }
57
}
58
 
59
void
60
GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude,
61
            float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
62
{
63
}
64
 
65
// XXX this is probably the wrong way to do it, too
66
void
67
GPS::_error(const char *msg)
68
{
69
    Serial.println(msg);
70
}