|
| 1 | +#include "WProgram.h" |
| 2 | +#include "NTP.h" |
| 3 | + |
| 4 | +NTP::NTP(byte address[]) |
| 5 | +{ |
| 6 | + _address[0] = address[0]; |
| 7 | + _address[1] = address[1]; |
| 8 | + _address[2] = address[2]; |
| 9 | + _address[3] = address[3]; |
| 10 | +} |
| 11 | + |
| 12 | +NTP::~NTP() |
| 13 | +{ |
| 14 | +} |
| 15 | + |
| 16 | +unsigned long NTP::get_time() |
| 17 | +{ |
| 18 | + call(); |
| 19 | + return _send_timestamp; |
| 20 | +} |
| 21 | + |
| 22 | +int NTP::get_leap_indicator(byte b) |
| 23 | +{ |
| 24 | + return b >> 6; |
| 25 | +} |
| 26 | + |
| 27 | +int NTP::get_version(byte b) |
| 28 | +{ |
| 29 | + byte c = b << 2; |
| 30 | + return c >> 5; |
| 31 | +} |
| 32 | + |
| 33 | +int NTP::get_mode(byte b) |
| 34 | +{ |
| 35 | + byte c = b << 5; |
| 36 | + return c >> 5; |
| 37 | +} |
| 38 | + |
| 39 | +unsigned long NTP::get_ulong() |
| 40 | +{ |
| 41 | + unsigned long ulong = (unsigned long)UdpBytewise.read() << 24; |
| 42 | + ulong |= (unsigned long)UdpBytewise.read() << 16; |
| 43 | + ulong |= (unsigned long)UdpBytewise.read() << 8; |
| 44 | + ulong |= (unsigned long)UdpBytewise.read(); |
| 45 | + return ulong; |
| 46 | +} |
| 47 | + |
| 48 | +unsigned long NTP::get_time_discard_precision() |
| 49 | +{ |
| 50 | + unsigned long time = get_ulong(); |
| 51 | + // we are going to discard the sub-second stuff |
| 52 | + UdpBytewise.read(); |
| 53 | + UdpBytewise.read(); |
| 54 | + UdpBytewise.read(); |
| 55 | + UdpBytewise.read(); |
| 56 | + return time; |
| 57 | +} |
| 58 | + |
| 59 | +void NTP::write_n(int what, int how_many) |
| 60 | +{ |
| 61 | + for( int i = 0; i < how_many; i++ ) |
| 62 | + UdpBytewise.write(what); |
| 63 | +} |
| 64 | + |
| 65 | +int NTP::send_ntp_packet() |
| 66 | +{ |
| 67 | + UdpBytewise.begin(123); |
| 68 | + UdpBytewise.beginPacket(_address, 123); |
| 69 | + // LI, Version, Mode |
| 70 | + UdpBytewise.write(B11100011); |
| 71 | + // Stratum |
| 72 | + UdpBytewise.write(0); |
| 73 | + // Polling Interval |
| 74 | + UdpBytewise.write(6); |
| 75 | + // Peer Clock Precision |
| 76 | + UdpBytewise.write(0xEC); |
| 77 | + // Root Delay |
| 78 | + write_n(0, 4); |
| 79 | + // Root Dispersion |
| 80 | + write_n(0, 4); |
| 81 | + // Reference Clock Id |
| 82 | + UdpBytewise.write(49); |
| 83 | + UdpBytewise.write(0x4E); |
| 84 | + UdpBytewise.write(49); |
| 85 | + UdpBytewise.write(52); |
| 86 | + // Reference CLock Update Time |
| 87 | + write_n(0, 8); |
| 88 | + // Originate Time Stamp |
| 89 | + write_n(0, 8); |
| 90 | + // Receive Time Stamp |
| 91 | + write_n(0, 8); |
| 92 | + // Transmit Time Stamp |
| 93 | + write_n(0, 8); |
| 94 | + // End |
| 95 | + return UdpBytewise.endPacket(); |
| 96 | +} |
| 97 | + |
| 98 | +void NTP::call() |
| 99 | +{ |
| 100 | + send_ntp_packet(); |
| 101 | + |
| 102 | + delay(1000); |
| 103 | + |
| 104 | + if ( UdpBytewise.available() ) { |
| 105 | + byte first_byte = UdpBytewise.read(); |
| 106 | + _leap_indicator = get_leap_indicator(first_byte); |
| 107 | + _version = get_version(first_byte); |
| 108 | + _mode = get_mode(first_byte); |
| 109 | + _stratum = (int)UdpBytewise.read(); |
| 110 | + _polling_interval = 1 << ((int)UdpBytewise.read()); |
| 111 | + _precision = (char)UdpBytewise.read(); |
| 112 | + _delay_interval = (float) get_ulong(); |
| 113 | + _dispersion = (float) get_ulong(); |
| 114 | + |
| 115 | + _ref_clock_id[0] = UdpBytewise.read(); |
| 116 | + _ref_clock_id[1] = UdpBytewise.read(); |
| 117 | + _ref_clock_id[2] = UdpBytewise.read(); |
| 118 | + _ref_clock_id[3] = UdpBytewise.read(); |
| 119 | + |
| 120 | + _ref_clock_update_time = get_time_discard_precision(); |
| 121 | + _orig_timestamp = get_time_discard_precision(); |
| 122 | + _recv_timestamp = get_time_discard_precision(); |
| 123 | + _send_timestamp = get_time_discard_precision(); |
| 124 | + } |
| 125 | +} |
0 commit comments