← Back to branch summary

~thomas-voss/location-service/make-baud-rate-configurable

« back to all changes in this revision

Viewing changes to src/location/providers/ubx/_8/scanner.cpp

Merge lp:~thomas-voss/location-service/enable-ublox-protocol.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/location/providers/ubx/_8/scanner.cpp
 
1
// Copyright (C) 2016 Thomas Voss <thomas.voss.bochum@gmail.com>
 
2
//
 
3
// This library is free software: you can redistribute it and/or modify
 
4
// it under the terms of the GNU Lesser General Public License as published
 
5
// by the Free Software Foundation, either version 3 of the License, or
 
6
// (at your option) any later version.
 
7
//
 
8
// This program is distributed in the hope that it will be useful,
 
9
// but WITHOUT ANY WARRANTY; without even the implied warranty of
 
10
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
11
// GNU General Public License for more details.
 
12
//
 
13
// You should have received a copy of the GNU Lesser General Public License
 
14
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
15
 
 
16
#include <location/providers/ubx/_8/scanner.h>
 
17
 
 
18
#include <location/providers/ubx/_8/codec.h>
 
19
#include <location/providers/ubx/_8/magic.h>
 
20
#include <location/providers/ubx/_8/message.h>
 
21
 
 
22
#include <cstdint>
 
23
#include <functional>
 
24
#include <iostream>
 
25
#include <map>
 
26
#include <stdexcept>
 
27
#include <tuple>
 
28
#include <vector>
 
29
 
 
30
namespace _8 = location::providers::ubx::_8;
 
31
 
 
32
namespace
 
33
{
 
34
 
 
35
template<std::uint8_t class_id, std::uint8_t message_id>
 
36
std::tuple<std::uint8_t, std::uint8_t> make_tuple()
 
37
{
 
38
    return std::make_tuple(class_id, message_id);
 
39
}
 
40
 
 
41
}  // namespace
 
42
 
 
43
_8::Scanner::Scanner()
 
44
    : next{Expect::sync_char_1},
 
45
      class_id{0},
 
46
      message_id{0},
 
47
      expected_size{0},
 
48
      payload_iterator{payload.end()}
 
49
{
 
50
    factories[make_tuple<ack::Ack::class_id, ack::Ack::message_id>()] =
 
51
            [](const std::vector<std::uint8_t>& payload) { return decode_message<ack::Ack>(payload); };
 
52
    factories[make_tuple<ack::Nak::class_id, ack::Nak::message_id>()] =
 
53
            [](const std::vector<std::uint8_t>& payload) { return decode_message<ack::Nak>(payload); };
 
54
    factories[make_tuple<cfg::Gnss::class_id, cfg::Gnss::message_id>()] =
 
55
            [](const std::vector<std::uint8_t>&) { return cfg::Gnss{}; };
 
56
    factories[make_tuple<cfg::Msg::class_id, cfg::Msg::message_id>()] =
 
57
            [](const std::vector<std::uint8_t>&) { return cfg::Msg{}; };
 
58
    factories[make_tuple<nav::Pvt::class_id, nav::Pvt::message_id>()] =
 
59
            [](const std::vector<std::uint8_t>& payload) { return decode_message<nav::Pvt>(payload); };
 
60
    factories[make_tuple<nav::Sat::class_id, nav::Sat::message_id>()] =
 
61
            [](const std::vector<std::uint8_t>& payload) { return decode_message<nav::Sat>(payload); };
 
62
}
 
63
 
 
64
std::tuple<_8::Scanner::Expect, bool> _8::Scanner::update(std::uint8_t c)
 
65
{
 
66
    bool consumed = false;
 
67
 
 
68
    // TODO(tvoss): This lacks a lot of validiation and verification.
 
69
    // UBX allows us to partially parse while we scan and carry out online
 
70
    // checksum calculation. Ideally, we would have a common class State that
 
71
    // captures behavior and transition logic.
 
72
    switch (next)
 
73
    {
 
74
    case Expect::sync_char_1:
 
75
        if (c == sync_char_1)
 
76
        {
 
77
            next = Expect::sync_char_2;
 
78
            consumed = true;
 
79
        }
 
80
        break;
 
81
    case Expect::sync_char_2:
 
82
        if (c == sync_char_2)
 
83
        {
 
84
            next = Expect::class_;
 
85
            consumed = true;
 
86
        }
 
87
        break;
 
88
    case Expect::class_:
 
89
        checksum(c);
 
90
        class_id = c;
 
91
        next = Expect::id;
 
92
        consumed = true;
 
93
        break;
 
94
    case Expect::id:
 
95
        checksum(c);
 
96
        message_id = c;
 
97
        next = Expect::length_1;
 
98
        consumed = true;
 
99
        break;
 
100
    case Expect::length_1:
 
101
        checksum(c);
 
102
        expected_size = c;
 
103
        next = Expect::length_2;
 
104
        consumed = true;
 
105
        break;
 
106
    case Expect::length_2:
 
107
        checksum(c);
 
108
        expected_size |= (c << 8);
 
109
        payload.resize(expected_size);
 
110
        payload_iterator = payload.begin();
 
111
        next = Expect::payload;
 
112
        consumed = true;
 
113
        break;
 
114
    case Expect::payload:
 
115
        checksum(c);
 
116
        *payload_iterator = c;
 
117
        ++payload_iterator;
 
118
        next = payload_iterator == payload.end()
 
119
                ? Expect::ck_a
 
120
                : next;
 
121
        consumed = true;
 
122
        break;
 
123
    case Expect::ck_a:
 
124
        ck_a = c;
 
125
        next = Expect::ck_b;
 
126
        consumed = true;
 
127
        break;
 
128
    case Expect::ck_b:
 
129
        ck_b = c;
 
130
        next = Expect::nothing_more;
 
131
        consumed = true;
 
132
        break;
 
133
    default:
 
134
        consumed = false;
 
135
        break;
 
136
    }
 
137
 
 
138
    return std::make_tuple(next, consumed);
 
139
}
 
140
 
 
141
_8::Message _8::Scanner::finalize()
 
142
{
 
143
    if (next != Expect::nothing_more)
 
144
        throw std::logic_error{"Not ready for extraction."};
 
145
 
 
146
    if (ck_a != checksum.ck_a() || ck_b != checksum.ck_b())
 
147
        throw std::runtime_error{"Verification failed."};
 
148
 
 
149
    auto it = factories.find(std::make_tuple(class_id, message_id));
 
150
 
 
151
    if (it == factories.end())
 
152
        throw std::runtime_error{"Could not decode message."};
 
153
 
 
154
    auto result = it->second(payload);
 
155
 
 
156
    checksum = Checksum{};
 
157
    next = Expect::sync_char_1;
 
158
    class_id = 0;
 
159
    message_id = 0;
 
160
    expected_size = 0;
 
161
    payload.clear();
 
162
    payload_iterator = payload.end();
 
163
    ck_a = ck_b = 0;
 
164
 
 
165
    return result;
 
166
}