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 | } |