├── .gitignore
├── .gitmodules
├── CMakeLists.txt
├── LICENSE
├── README.md
├── cmake
└── FindReadline.cmake
├── example
├── bcasttest.conf
├── sitl2gcs.conf
└── test.conf
└── src
├── asyncsocket.cpp
├── asyncsocket.h
├── configfile.cpp
├── configfile.h
├── exception.h
├── main.cpp
├── mavhelper.h
├── mlink.cpp
├── mlink.h
├── serial.cpp
├── serial.h
├── shell.cpp
└── shell.h
/.gitignore:
--------------------------------------------------------------------------------
1 | # Compiled Object files
2 | *.slo
3 | *.lo
4 | *.o
5 | *.obj
6 |
7 | # Precompiled Headers
8 | *.gch
9 | *.pch
10 |
11 | # Compiled Dynamic libraries
12 | *.so
13 | *.dylib
14 | *.dll
15 |
16 | # Fortran module files
17 | *.mod
18 |
19 | # Compiled Static libraries
20 | *.lai
21 | *.la
22 | *.a
23 | *.lib
24 |
25 | # Executables
26 | *.exe
27 | *.out
28 | *.app
29 |
30 | # Build directory
31 | build/
32 |
33 | # astyle leftovers
34 | *.orig
35 |
--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "include/mavlink2"]
2 | path = include/mavlink2
3 | url = https://github.com/mavlink/c_library_v2.git
4 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8)
2 | project("cmavnode" C CXX)
3 |
4 | if(NOT CMAKE_BUILD_TYPE)
5 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING
6 | "Choose the type of build, options are: Debug, Release."
7 | FORCE)
8 | endif(NOT CMAKE_BUILD_TYPE)
9 |
10 | # Include find files
11 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
12 |
13 | set(Boost_USE_STATIC_LIBS ON)
14 |
15 | find_package( Boost 1.40 COMPONENTS program_options thread system REQUIRED )
16 | find_package( Threads REQUIRED)
17 | find_package( Readline REQUIRED)
18 |
19 | INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} )
20 | INCLUDE_DIRECTORIES( ${READLINE_INCLUDE_DIR})
21 |
22 | # glob source files
23 | file(GLOB cmavnode_SRC
24 | "src/*.cpp"
25 | )
26 |
27 | #actual executable
28 | add_executable(cmavnode ${cmavnode_SRC})
29 | set(CMAKE_CXX_FLAGS_RELEASE "-DNDEBUG")
30 | set(CMAKE_CXX_FLAGS_DEBUG " -ggdb")
31 | set(CMAKE_CXX_FLAGS "-std=c++11 -Wno-address-of-packed-member -DMAVLINK_USE_MESSAGE_INFO")
32 |
33 | TARGET_LINK_LIBRARIES(cmavnode ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${READLINE_LIBRARY})
34 |
35 | install(TARGETS cmavnode DESTINATION bin)
36 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | GNU GENERAL PUBLIC LICENSE
2 | Version 3, 29 June 2007
3 |
4 | Copyright (C) 2007 Free Software Foundation, Inc.
5 | Everyone is permitted to copy and distribute verbatim copies
6 | of this license document, but changing it is not allowed.
7 |
8 | Preamble
9 |
10 | The GNU General Public License is a free, copyleft license for
11 | software and other kinds of works.
12 |
13 | The licenses for most software and other practical works are designed
14 | to take away your freedom to share and change the works. By contrast,
15 | the GNU General Public License is intended to guarantee your freedom to
16 | share and change all versions of a program--to make sure it remains free
17 | software for all its users. We, the Free Software Foundation, use the
18 | GNU General Public License for most of our software; it applies also to
19 | any other work released this way by its authors. You can apply it to
20 | your programs, too.
21 |
22 | When we speak of free software, we are referring to freedom, not
23 | price. Our General Public Licenses are designed to make sure that you
24 | have the freedom to distribute copies of free software (and charge for
25 | them if you wish), that you receive source code or can get it if you
26 | want it, that you can change the software or use pieces of it in new
27 | free programs, and that you know you can do these things.
28 |
29 | To protect your rights, we need to prevent others from denying you
30 | these rights or asking you to surrender the rights. Therefore, you have
31 | certain responsibilities if you distribute copies of the software, or if
32 | you modify it: responsibilities to respect the freedom of others.
33 |
34 | For example, if you distribute copies of such a program, whether
35 | gratis or for a fee, you must pass on to the recipients the same
36 | freedoms that you received. You must make sure that they, too, receive
37 | or can get the source code. And you must show them these terms so they
38 | know their rights.
39 |
40 | Developers that use the GNU GPL protect your rights with two steps:
41 | (1) assert copyright on the software, and (2) offer you this License
42 | giving you legal permission to copy, distribute and/or modify it.
43 |
44 | For the developers' and authors' protection, the GPL clearly explains
45 | that there is no warranty for this free software. For both users' and
46 | authors' sake, the GPL requires that modified versions be marked as
47 | changed, so that their problems will not be attributed erroneously to
48 | authors of previous versions.
49 |
50 | Some devices are designed to deny users access to install or run
51 | modified versions of the software inside them, although the manufacturer
52 | can do so. This is fundamentally incompatible with the aim of
53 | protecting users' freedom to change the software. The systematic
54 | pattern of such abuse occurs in the area of products for individuals to
55 | use, which is precisely where it is most unacceptable. Therefore, we
56 | have designed this version of the GPL to prohibit the practice for those
57 | products. If such problems arise substantially in other domains, we
58 | stand ready to extend this provision to those domains in future versions
59 | of the GPL, as needed to protect the freedom of users.
60 |
61 | Finally, every program is threatened constantly by software patents.
62 | States should not allow patents to restrict development and use of
63 | software on general-purpose computers, but in those that do, we wish to
64 | avoid the special danger that patents applied to a free program could
65 | make it effectively proprietary. To prevent this, the GPL assures that
66 | patents cannot be used to render the program non-free.
67 |
68 | The precise terms and conditions for copying, distribution and
69 | modification follow.
70 |
71 | TERMS AND CONDITIONS
72 |
73 | 0. Definitions.
74 |
75 | "This License" refers to version 3 of the GNU General Public License.
76 |
77 | "Copyright" also means copyright-like laws that apply to other kinds of
78 | works, such as semiconductor masks.
79 |
80 | "The Program" refers to any copyrightable work licensed under this
81 | License. Each licensee is addressed as "you". "Licensees" and
82 | "recipients" may be individuals or organizations.
83 |
84 | To "modify" a work means to copy from or adapt all or part of the work
85 | in a fashion requiring copyright permission, other than the making of an
86 | exact copy. The resulting work is called a "modified version" of the
87 | earlier work or a work "based on" the earlier work.
88 |
89 | A "covered work" means either the unmodified Program or a work based
90 | on the Program.
91 |
92 | To "propagate" a work means to do anything with it that, without
93 | permission, would make you directly or secondarily liable for
94 | infringement under applicable copyright law, except executing it on a
95 | computer or modifying a private copy. Propagation includes copying,
96 | distribution (with or without modification), making available to the
97 | public, and in some countries other activities as well.
98 |
99 | To "convey" a work means any kind of propagation that enables other
100 | parties to make or receive copies. Mere interaction with a user through
101 | a computer network, with no transfer of a copy, is not conveying.
102 |
103 | An interactive user interface displays "Appropriate Legal Notices"
104 | to the extent that it includes a convenient and prominently visible
105 | feature that (1) displays an appropriate copyright notice, and (2)
106 | tells the user that there is no warranty for the work (except to the
107 | extent that warranties are provided), that licensees may convey the
108 | work under this License, and how to view a copy of this License. If
109 | the interface presents a list of user commands or options, such as a
110 | menu, a prominent item in the list meets this criterion.
111 |
112 | 1. Source Code.
113 |
114 | The "source code" for a work means the preferred form of the work
115 | for making modifications to it. "Object code" means any non-source
116 | form of a work.
117 |
118 | A "Standard Interface" means an interface that either is an official
119 | standard defined by a recognized standards body, or, in the case of
120 | interfaces specified for a particular programming language, one that
121 | is widely used among developers working in that language.
122 |
123 | The "System Libraries" of an executable work include anything, other
124 | than the work as a whole, that (a) is included in the normal form of
125 | packaging a Major Component, but which is not part of that Major
126 | Component, and (b) serves only to enable use of the work with that
127 | Major Component, or to implement a Standard Interface for which an
128 | implementation is available to the public in source code form. A
129 | "Major Component", in this context, means a major essential component
130 | (kernel, window system, and so on) of the specific operating system
131 | (if any) on which the executable work runs, or a compiler used to
132 | produce the work, or an object code interpreter used to run it.
133 |
134 | The "Corresponding Source" for a work in object code form means all
135 | the source code needed to generate, install, and (for an executable
136 | work) run the object code and to modify the work, including scripts to
137 | control those activities. However, it does not include the work's
138 | System Libraries, or general-purpose tools or generally available free
139 | programs which are used unmodified in performing those activities but
140 | which are not part of the work. For example, Corresponding Source
141 | includes interface definition files associated with source files for
142 | the work, and the source code for shared libraries and dynamically
143 | linked subprograms that the work is specifically designed to require,
144 | such as by intimate data communication or control flow between those
145 | subprograms and other parts of the work.
146 |
147 | The Corresponding Source need not include anything that users
148 | can regenerate automatically from other parts of the Corresponding
149 | Source.
150 |
151 | The Corresponding Source for a work in source code form is that
152 | same work.
153 |
154 | 2. Basic Permissions.
155 |
156 | All rights granted under this License are granted for the term of
157 | copyright on the Program, and are irrevocable provided the stated
158 | conditions are met. This License explicitly affirms your unlimited
159 | permission to run the unmodified Program. The output from running a
160 | covered work is covered by this License only if the output, given its
161 | content, constitutes a covered work. This License acknowledges your
162 | rights of fair use or other equivalent, as provided by copyright law.
163 |
164 | You may make, run and propagate covered works that you do not
165 | convey, without conditions so long as your license otherwise remains
166 | in force. You may convey covered works to others for the sole purpose
167 | of having them make modifications exclusively for you, or provide you
168 | with facilities for running those works, provided that you comply with
169 | the terms of this License in conveying all material for which you do
170 | not control copyright. Those thus making or running the covered works
171 | for you must do so exclusively on your behalf, under your direction
172 | and control, on terms that prohibit them from making any copies of
173 | your copyrighted material outside their relationship with you.
174 |
175 | Conveying under any other circumstances is permitted solely under
176 | the conditions stated below. Sublicensing is not allowed; section 10
177 | makes it unnecessary.
178 |
179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
180 |
181 | No covered work shall be deemed part of an effective technological
182 | measure under any applicable law fulfilling obligations under article
183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or
184 | similar laws prohibiting or restricting circumvention of such
185 | measures.
186 |
187 | When you convey a covered work, you waive any legal power to forbid
188 | circumvention of technological measures to the extent such circumvention
189 | is effected by exercising rights under this License with respect to
190 | the covered work, and you disclaim any intention to limit operation or
191 | modification of the work as a means of enforcing, against the work's
192 | users, your or third parties' legal rights to forbid circumvention of
193 | technological measures.
194 |
195 | 4. Conveying Verbatim Copies.
196 |
197 | You may convey verbatim copies of the Program's source code as you
198 | receive it, in any medium, provided that you conspicuously and
199 | appropriately publish on each copy an appropriate copyright notice;
200 | keep intact all notices stating that this License and any
201 | non-permissive terms added in accord with section 7 apply to the code;
202 | keep intact all notices of the absence of any warranty; and give all
203 | recipients a copy of this License along with the Program.
204 |
205 | You may charge any price or no price for each copy that you convey,
206 | and you may offer support or warranty protection for a fee.
207 |
208 | 5. Conveying Modified Source Versions.
209 |
210 | You may convey a work based on the Program, or the modifications to
211 | produce it from the Program, in the form of source code under the
212 | terms of section 4, provided that you also meet all of these conditions:
213 |
214 | a) The work must carry prominent notices stating that you modified
215 | it, and giving a relevant date.
216 |
217 | b) The work must carry prominent notices stating that it is
218 | released under this License and any conditions added under section
219 | 7. This requirement modifies the requirement in section 4 to
220 | "keep intact all notices".
221 |
222 | c) You must license the entire work, as a whole, under this
223 | License to anyone who comes into possession of a copy. This
224 | License will therefore apply, along with any applicable section 7
225 | additional terms, to the whole of the work, and all its parts,
226 | regardless of how they are packaged. This License gives no
227 | permission to license the work in any other way, but it does not
228 | invalidate such permission if you have separately received it.
229 |
230 | d) If the work has interactive user interfaces, each must display
231 | Appropriate Legal Notices; however, if the Program has interactive
232 | interfaces that do not display Appropriate Legal Notices, your
233 | work need not make them do so.
234 |
235 | A compilation of a covered work with other separate and independent
236 | works, which are not by their nature extensions of the covered work,
237 | and which are not combined with it such as to form a larger program,
238 | in or on a volume of a storage or distribution medium, is called an
239 | "aggregate" if the compilation and its resulting copyright are not
240 | used to limit the access or legal rights of the compilation's users
241 | beyond what the individual works permit. Inclusion of a covered work
242 | in an aggregate does not cause this License to apply to the other
243 | parts of the aggregate.
244 |
245 | 6. Conveying Non-Source Forms.
246 |
247 | You may convey a covered work in object code form under the terms
248 | of sections 4 and 5, provided that you also convey the
249 | machine-readable Corresponding Source under the terms of this License,
250 | in one of these ways:
251 |
252 | a) Convey the object code in, or embodied in, a physical product
253 | (including a physical distribution medium), accompanied by the
254 | Corresponding Source fixed on a durable physical medium
255 | customarily used for software interchange.
256 |
257 | b) Convey the object code in, or embodied in, a physical product
258 | (including a physical distribution medium), accompanied by a
259 | written offer, valid for at least three years and valid for as
260 | long as you offer spare parts or customer support for that product
261 | model, to give anyone who possesses the object code either (1) a
262 | copy of the Corresponding Source for all the software in the
263 | product that is covered by this License, on a durable physical
264 | medium customarily used for software interchange, for a price no
265 | more than your reasonable cost of physically performing this
266 | conveying of source, or (2) access to copy the
267 | Corresponding Source from a network server at no charge.
268 |
269 | c) Convey individual copies of the object code with a copy of the
270 | written offer to provide the Corresponding Source. This
271 | alternative is allowed only occasionally and noncommercially, and
272 | only if you received the object code with such an offer, in accord
273 | with subsection 6b.
274 |
275 | d) Convey the object code by offering access from a designated
276 | place (gratis or for a charge), and offer equivalent access to the
277 | Corresponding Source in the same way through the same place at no
278 | further charge. You need not require recipients to copy the
279 | Corresponding Source along with the object code. If the place to
280 | copy the object code is a network server, the Corresponding Source
281 | may be on a different server (operated by you or a third party)
282 | that supports equivalent copying facilities, provided you maintain
283 | clear directions next to the object code saying where to find the
284 | Corresponding Source. Regardless of what server hosts the
285 | Corresponding Source, you remain obligated to ensure that it is
286 | available for as long as needed to satisfy these requirements.
287 |
288 | e) Convey the object code using peer-to-peer transmission, provided
289 | you inform other peers where the object code and Corresponding
290 | Source of the work are being offered to the general public at no
291 | charge under subsection 6d.
292 |
293 | A separable portion of the object code, whose source code is excluded
294 | from the Corresponding Source as a System Library, need not be
295 | included in conveying the object code work.
296 |
297 | A "User Product" is either (1) a "consumer product", which means any
298 | tangible personal property which is normally used for personal, family,
299 | or household purposes, or (2) anything designed or sold for incorporation
300 | into a dwelling. In determining whether a product is a consumer product,
301 | doubtful cases shall be resolved in favor of coverage. For a particular
302 | product received by a particular user, "normally used" refers to a
303 | typical or common use of that class of product, regardless of the status
304 | of the particular user or of the way in which the particular user
305 | actually uses, or expects or is expected to use, the product. A product
306 | is a consumer product regardless of whether the product has substantial
307 | commercial, industrial or non-consumer uses, unless such uses represent
308 | the only significant mode of use of the product.
309 |
310 | "Installation Information" for a User Product means any methods,
311 | procedures, authorization keys, or other information required to install
312 | and execute modified versions of a covered work in that User Product from
313 | a modified version of its Corresponding Source. The information must
314 | suffice to ensure that the continued functioning of the modified object
315 | code is in no case prevented or interfered with solely because
316 | modification has been made.
317 |
318 | If you convey an object code work under this section in, or with, or
319 | specifically for use in, a User Product, and the conveying occurs as
320 | part of a transaction in which the right of possession and use of the
321 | User Product is transferred to the recipient in perpetuity or for a
322 | fixed term (regardless of how the transaction is characterized), the
323 | Corresponding Source conveyed under this section must be accompanied
324 | by the Installation Information. But this requirement does not apply
325 | if neither you nor any third party retains the ability to install
326 | modified object code on the User Product (for example, the work has
327 | been installed in ROM).
328 |
329 | The requirement to provide Installation Information does not include a
330 | requirement to continue to provide support service, warranty, or updates
331 | for a work that has been modified or installed by the recipient, or for
332 | the User Product in which it has been modified or installed. Access to a
333 | network may be denied when the modification itself materially and
334 | adversely affects the operation of the network or violates the rules and
335 | protocols for communication across the network.
336 |
337 | Corresponding Source conveyed, and Installation Information provided,
338 | in accord with this section must be in a format that is publicly
339 | documented (and with an implementation available to the public in
340 | source code form), and must require no special password or key for
341 | unpacking, reading or copying.
342 |
343 | 7. Additional Terms.
344 |
345 | "Additional permissions" are terms that supplement the terms of this
346 | License by making exceptions from one or more of its conditions.
347 | Additional permissions that are applicable to the entire Program shall
348 | be treated as though they were included in this License, to the extent
349 | that they are valid under applicable law. If additional permissions
350 | apply only to part of the Program, that part may be used separately
351 | under those permissions, but the entire Program remains governed by
352 | this License without regard to the additional permissions.
353 |
354 | When you convey a copy of a covered work, you may at your option
355 | remove any additional permissions from that copy, or from any part of
356 | it. (Additional permissions may be written to require their own
357 | removal in certain cases when you modify the work.) You may place
358 | additional permissions on material, added by you to a covered work,
359 | for which you have or can give appropriate copyright permission.
360 |
361 | Notwithstanding any other provision of this License, for material you
362 | add to a covered work, you may (if authorized by the copyright holders of
363 | that material) supplement the terms of this License with terms:
364 |
365 | a) Disclaiming warranty or limiting liability differently from the
366 | terms of sections 15 and 16 of this License; or
367 |
368 | b) Requiring preservation of specified reasonable legal notices or
369 | author attributions in that material or in the Appropriate Legal
370 | Notices displayed by works containing it; or
371 |
372 | c) Prohibiting misrepresentation of the origin of that material, or
373 | requiring that modified versions of such material be marked in
374 | reasonable ways as different from the original version; or
375 |
376 | d) Limiting the use for publicity purposes of names of licensors or
377 | authors of the material; or
378 |
379 | e) Declining to grant rights under trademark law for use of some
380 | trade names, trademarks, or service marks; or
381 |
382 | f) Requiring indemnification of licensors and authors of that
383 | material by anyone who conveys the material (or modified versions of
384 | it) with contractual assumptions of liability to the recipient, for
385 | any liability that these contractual assumptions directly impose on
386 | those licensors and authors.
387 |
388 | All other non-permissive additional terms are considered "further
389 | restrictions" within the meaning of section 10. If the Program as you
390 | received it, or any part of it, contains a notice stating that it is
391 | governed by this License along with a term that is a further
392 | restriction, you may remove that term. If a license document contains
393 | a further restriction but permits relicensing or conveying under this
394 | License, you may add to a covered work material governed by the terms
395 | of that license document, provided that the further restriction does
396 | not survive such relicensing or conveying.
397 |
398 | If you add terms to a covered work in accord with this section, you
399 | must place, in the relevant source files, a statement of the
400 | additional terms that apply to those files, or a notice indicating
401 | where to find the applicable terms.
402 |
403 | Additional terms, permissive or non-permissive, may be stated in the
404 | form of a separately written license, or stated as exceptions;
405 | the above requirements apply either way.
406 |
407 | 8. Termination.
408 |
409 | You may not propagate or modify a covered work except as expressly
410 | provided under this License. Any attempt otherwise to propagate or
411 | modify it is void, and will automatically terminate your rights under
412 | this License (including any patent licenses granted under the third
413 | paragraph of section 11).
414 |
415 | However, if you cease all violation of this License, then your
416 | license from a particular copyright holder is reinstated (a)
417 | provisionally, unless and until the copyright holder explicitly and
418 | finally terminates your license, and (b) permanently, if the copyright
419 | holder fails to notify you of the violation by some reasonable means
420 | prior to 60 days after the cessation.
421 |
422 | Moreover, your license from a particular copyright holder is
423 | reinstated permanently if the copyright holder notifies you of the
424 | violation by some reasonable means, this is the first time you have
425 | received notice of violation of this License (for any work) from that
426 | copyright holder, and you cure the violation prior to 30 days after
427 | your receipt of the notice.
428 |
429 | Termination of your rights under this section does not terminate the
430 | licenses of parties who have received copies or rights from you under
431 | this License. If your rights have been terminated and not permanently
432 | reinstated, you do not qualify to receive new licenses for the same
433 | material under section 10.
434 |
435 | 9. Acceptance Not Required for Having Copies.
436 |
437 | You are not required to accept this License in order to receive or
438 | run a copy of the Program. Ancillary propagation of a covered work
439 | occurring solely as a consequence of using peer-to-peer transmission
440 | to receive a copy likewise does not require acceptance. However,
441 | nothing other than this License grants you permission to propagate or
442 | modify any covered work. These actions infringe copyright if you do
443 | not accept this License. Therefore, by modifying or propagating a
444 | covered work, you indicate your acceptance of this License to do so.
445 |
446 | 10. Automatic Licensing of Downstream Recipients.
447 |
448 | Each time you convey a covered work, the recipient automatically
449 | receives a license from the original licensors, to run, modify and
450 | propagate that work, subject to this License. You are not responsible
451 | for enforcing compliance by third parties with this License.
452 |
453 | An "entity transaction" is a transaction transferring control of an
454 | organization, or substantially all assets of one, or subdividing an
455 | organization, or merging organizations. If propagation of a covered
456 | work results from an entity transaction, each party to that
457 | transaction who receives a copy of the work also receives whatever
458 | licenses to the work the party's predecessor in interest had or could
459 | give under the previous paragraph, plus a right to possession of the
460 | Corresponding Source of the work from the predecessor in interest, if
461 | the predecessor has it or can get it with reasonable efforts.
462 |
463 | You may not impose any further restrictions on the exercise of the
464 | rights granted or affirmed under this License. For example, you may
465 | not impose a license fee, royalty, or other charge for exercise of
466 | rights granted under this License, and you may not initiate litigation
467 | (including a cross-claim or counterclaim in a lawsuit) alleging that
468 | any patent claim is infringed by making, using, selling, offering for
469 | sale, or importing the Program or any portion of it.
470 |
471 | 11. Patents.
472 |
473 | A "contributor" is a copyright holder who authorizes use under this
474 | License of the Program or a work on which the Program is based. The
475 | work thus licensed is called the contributor's "contributor version".
476 |
477 | A contributor's "essential patent claims" are all patent claims
478 | owned or controlled by the contributor, whether already acquired or
479 | hereafter acquired, that would be infringed by some manner, permitted
480 | by this License, of making, using, or selling its contributor version,
481 | but do not include claims that would be infringed only as a
482 | consequence of further modification of the contributor version. For
483 | purposes of this definition, "control" includes the right to grant
484 | patent sublicenses in a manner consistent with the requirements of
485 | this License.
486 |
487 | Each contributor grants you a non-exclusive, worldwide, royalty-free
488 | patent license under the contributor's essential patent claims, to
489 | make, use, sell, offer for sale, import and otherwise run, modify and
490 | propagate the contents of its contributor version.
491 |
492 | In the following three paragraphs, a "patent license" is any express
493 | agreement or commitment, however denominated, not to enforce a patent
494 | (such as an express permission to practice a patent or covenant not to
495 | sue for patent infringement). To "grant" such a patent license to a
496 | party means to make such an agreement or commitment not to enforce a
497 | patent against the party.
498 |
499 | If you convey a covered work, knowingly relying on a patent license,
500 | and the Corresponding Source of the work is not available for anyone
501 | to copy, free of charge and under the terms of this License, through a
502 | publicly available network server or other readily accessible means,
503 | then you must either (1) cause the Corresponding Source to be so
504 | available, or (2) arrange to deprive yourself of the benefit of the
505 | patent license for this particular work, or (3) arrange, in a manner
506 | consistent with the requirements of this License, to extend the patent
507 | license to downstream recipients. "Knowingly relying" means you have
508 | actual knowledge that, but for the patent license, your conveying the
509 | covered work in a country, or your recipient's use of the covered work
510 | in a country, would infringe one or more identifiable patents in that
511 | country that you have reason to believe are valid.
512 |
513 | If, pursuant to or in connection with a single transaction or
514 | arrangement, you convey, or propagate by procuring conveyance of, a
515 | covered work, and grant a patent license to some of the parties
516 | receiving the covered work authorizing them to use, propagate, modify
517 | or convey a specific copy of the covered work, then the patent license
518 | you grant is automatically extended to all recipients of the covered
519 | work and works based on it.
520 |
521 | A patent license is "discriminatory" if it does not include within
522 | the scope of its coverage, prohibits the exercise of, or is
523 | conditioned on the non-exercise of one or more of the rights that are
524 | specifically granted under this License. You may not convey a covered
525 | work if you are a party to an arrangement with a third party that is
526 | in the business of distributing software, under which you make payment
527 | to the third party based on the extent of your activity of conveying
528 | the work, and under which the third party grants, to any of the
529 | parties who would receive the covered work from you, a discriminatory
530 | patent license (a) in connection with copies of the covered work
531 | conveyed by you (or copies made from those copies), or (b) primarily
532 | for and in connection with specific products or compilations that
533 | contain the covered work, unless you entered into that arrangement,
534 | or that patent license was granted, prior to 28 March 2007.
535 |
536 | Nothing in this License shall be construed as excluding or limiting
537 | any implied license or other defenses to infringement that may
538 | otherwise be available to you under applicable patent law.
539 |
540 | 12. No Surrender of Others' Freedom.
541 |
542 | If conditions are imposed on you (whether by court order, agreement or
543 | otherwise) that contradict the conditions of this License, they do not
544 | excuse you from the conditions of this License. If you cannot convey a
545 | covered work so as to satisfy simultaneously your obligations under this
546 | License and any other pertinent obligations, then as a consequence you may
547 | not convey it at all. For example, if you agree to terms that obligate you
548 | to collect a royalty for further conveying from those to whom you convey
549 | the Program, the only way you could satisfy both those terms and this
550 | License would be to refrain entirely from conveying the Program.
551 |
552 | 13. Use with the GNU Affero General Public License.
553 |
554 | Notwithstanding any other provision of this License, you have
555 | permission to link or combine any covered work with a work licensed
556 | under version 3 of the GNU Affero General Public License into a single
557 | combined work, and to convey the resulting work. The terms of this
558 | License will continue to apply to the part which is the covered work,
559 | but the special requirements of the GNU Affero General Public License,
560 | section 13, concerning interaction through a network will apply to the
561 | combination as such.
562 |
563 | 14. Revised Versions of this License.
564 |
565 | The Free Software Foundation may publish revised and/or new versions of
566 | the GNU General Public License from time to time. Such new versions will
567 | be similar in spirit to the present version, but may differ in detail to
568 | address new problems or concerns.
569 |
570 | Each version is given a distinguishing version number. If the
571 | Program specifies that a certain numbered version of the GNU General
572 | Public License "or any later version" applies to it, you have the
573 | option of following the terms and conditions either of that numbered
574 | version or of any later version published by the Free Software
575 | Foundation. If the Program does not specify a version number of the
576 | GNU General Public License, you may choose any version ever published
577 | by the Free Software Foundation.
578 |
579 | If the Program specifies that a proxy can decide which future
580 | versions of the GNU General Public License can be used, that proxy's
581 | public statement of acceptance of a version permanently authorizes you
582 | to choose that version for the Program.
583 |
584 | Later license versions may give you additional or different
585 | permissions. However, no additional obligations are imposed on any
586 | author or copyright holder as a result of your choosing to follow a
587 | later version.
588 |
589 | 15. Disclaimer of Warranty.
590 |
591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
599 |
600 | 16. Limitation of Liability.
601 |
602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
610 | SUCH DAMAGES.
611 |
612 | 17. Interpretation of Sections 15 and 16.
613 |
614 | If the disclaimer of warranty and limitation of liability provided
615 | above cannot be given local legal effect according to their terms,
616 | reviewing courts shall apply local law that most closely approximates
617 | an absolute waiver of all civil liability in connection with the
618 | Program, unless a warranty or assumption of liability accompanies a
619 | copy of the Program in return for a fee.
620 |
621 | END OF TERMS AND CONDITIONS
622 |
623 | How to Apply These Terms to Your New Programs
624 |
625 | If you develop a new program, and you want it to be of the greatest
626 | possible use to the public, the best way to achieve this is to make it
627 | free software which everyone can redistribute and change under these terms.
628 |
629 | To do so, attach the following notices to the program. It is safest
630 | to attach them to the start of each source file to most effectively
631 | state the exclusion of warranty; and each file should have at least
632 | the "copyright" line and a pointer to where the full notice is found.
633 |
634 | {one line to give the program's name and a brief idea of what it does.}
635 | Copyright (C) {year} {name of author}
636 |
637 | This program is free software: you can redistribute it and/or modify
638 | it under the terms of the GNU General Public License as published by
639 | the Free Software Foundation, either version 3 of the License, or
640 | (at your option) any later version.
641 |
642 | This program is distributed in the hope that it will be useful,
643 | but WITHOUT ANY WARRANTY; without even the implied warranty of
644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
645 | GNU General Public License for more details.
646 |
647 | You should have received a copy of the GNU General Public License
648 | along with this program. If not, see .
649 |
650 | Also add information on how to contact you by electronic and paper mail.
651 |
652 | If the program does terminal interaction, make it output a short
653 | notice like this when it starts in an interactive mode:
654 |
655 | {project} Copyright (C) {year} {fullname}
656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
657 | This is free software, and you are welcome to redistribute it
658 | under certain conditions; type `show c' for details.
659 |
660 | The hypothetical commands `show w' and `show c' should show the appropriate
661 | parts of the General Public License. Of course, your program's commands
662 | might be different; for a GUI interface, you would use an "about box".
663 |
664 | You should also get your employer (if you work as a programmer) or school,
665 | if any, to sign a "copyright disclaimer" for the program, if necessary.
666 | For more information on this, and how to apply and follow the GNU GPL, see
667 | .
668 |
669 | The GNU General Public License does not permit incorporating your program
670 | into proprietary programs. If your program is a subroutine library, you
671 | may consider it more useful to permit linking proprietary applications with
672 | the library. If this is what you want to do, use the GNU Lesser General
673 | Public License instead of this License. But first, please read
674 | .
675 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # cmavnode
2 | MAVLink forwarding node written in C++
3 |
4 | This program can forward packets between an arbitrary number of MAVLink connections.
5 | Supports UDP, and Serial.
6 |
7 | Mavlink routing is done transparently. (cmavnode will not inject any packets)
8 | cmavnode treats each link equally unless specified otherwise, and does not differentiate between an autopilot and a groundstation.
9 |
10 | ## Installing
11 |
12 | - Clone the repository
13 |
14 | - Update Git submodules
15 |
16 | git submodule update --init
17 |
18 | - Install dependencies
19 |
20 | Ubuntu 14.04: sudo apt-get install libboost-all-dev cmake libconfig++ libreadline-dev
21 | Ubuntu 16.04: sudo apt-get install libboost-all-dev cmake libconfig++-dev libreadline-dev
22 | Debian Stretch: sudo apt-get install libboost-all-dev cmake libconfig++-dev libreadline-dev
23 | * Build cmavnode
24 |
25 | mkdir build && cd build
26 | cmake ..
27 | make
28 | sudo make install
29 |
30 | ## Usage
31 |
32 | ./cmavnode -f
33 |
34 | cmavnode is configured by a config file. This config file specifies the links you want (socket or serial) and allows you to dictate routing rules. Please see examples to see how this works.
35 |
36 | Use -i to get an interactive shell, type help into the shell to list commands.
37 |
38 | ## Config File
39 | cmavnode uses a config file which defines the links it should create. Each link has several options, some of which are optional.
40 |
41 | A link is defined by the following syntax
42 |
43 | [linkname]
44 | option1=.....
45 | option2=.....
46 |
47 | ### Serial Port
48 | This is the minimum configuration needed for a serial port
49 |
50 | [linkname]
51 | type=serial
52 | port=/dev/ttyUSB0
53 | baud=57600
54 |
55 | By default hardware flow control (RTS,CTS) is disabled, to enable it use:
56 |
57 | flow_control=true
58 |
59 | ### UDP
60 | UDP can operate in different ways.
61 |
62 | #### Fully Specified
63 | The first simple way is to specify all the connection information, as follows.
64 |
65 | [linkname]
66 | type=socket
67 | targetip=127.0.0.1
68 | targetport=14553
69 | localport=14550
70 |
71 | #### UDP Server
72 | Specify only the localport. Cmavnode will lock onto the first endpoint that sends to it.
73 |
74 | [linkname]
75 | type=socket
76 | localport=14550
77 |
78 | #### UDP Client
79 | Specify only targetip and targetport, and the local port will be asigned by the kernel. If you don't specify target ip it will default to "localhost".
80 |
81 | [linkname]
82 | type=socket
83 | targetip=192.168.1.1
84 | targetport=14550
85 |
86 | #### UDP Broadcast
87 | Will broadcast to a specified broadcast address using a specified port. You can choose to bind on a specific ip rather than 0.0.0.0, this will only affect whether you receive from all interfaces.
88 | By default the link will broadcast to every device, until a device responds, then the link will stop broadcasting and turn into a normal udp socket with the device. (Like mavproxy does with udpbcast option)
89 | If you want to keep broadcasting and support connections to multiple devices, set bcastlock to false.
90 |
91 | [linkname]
92 | type=udpbcast
93 | bcastip=192.168.0.255
94 | bcastport=14553
95 | bcastlock=false #optional, default true
96 | bindip=192.168.0.30 #optional, default 0.0.0.0
97 |
98 | ### Optional Flags
99 | The following flags can be applied to any type of link and are optional
100 |
101 | sim_enable=true #enables simulation options
102 | sim_packet_loss=25 #simulates packet loss of 25% on this link (incoming and outgoing)
103 | output_only_from=1,2,3 #only sends packets from sysID's 1, 2, and 3 on this link
104 | reject_repeat_packets=true #enables detection and removal of duplicate packets from different links
105 | sik_radio=true #enable this to be able to see radio stats (rssi, noise etc) on the console interface
106 | sleep=true #dont output to this link unless packets have been recently received (reduce wasted traffic on LTE/Satcomm)
107 | filter=DROP:HEARTBEART #exclusive ouput message filter, dont output heartbeat packets on this link
108 | filter=ACCEPT:HEARTBEAT,GLOBAL_POSITION_INT #inclusive output message filter, only output heartbeat and global position int messages on this link
109 |
110 |
111 | ## Licence
112 | Cmavnode is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
113 |
114 | Cmavnode is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
115 |
116 | You should have received a copy of the GNU General Public License along with Cmavnode. If not, see http://www.gnu.org/licenses/.
117 |
--------------------------------------------------------------------------------
/cmake/FindReadline.cmake:
--------------------------------------------------------------------------------
1 | # Module to find libreadline
2 | # To get readline on debian or derivitave install libreadline-dev
3 |
4 | # Will define:
5 | # READLINE_INCLUDE_DIR
6 | # READLINE_LIBRARY
7 | # READLINE_FOUND
8 | FIND_PATH(READLINE_INCLUDE_DIR readline/readline.h)
9 | FIND_LIBRARY(READLINE_LIBRARY NAMES readline)
10 |
11 | IF (READLINE_INCLUDE_DIR AND READLINE_LIBRARY)
12 | SET(READLINE_FOUND TRUE)
13 | MESSAGE(STATUS "Found Readline library at ${READLINE_LIBRARY}")
14 | MESSAGE(STATUS "Readline headers found at at ${READLINE_INCLUDE_DIR}")
15 | INCLUDE_DIRECTORIES(${READLINE_INCLUDE_DIR})
16 | ELSE (READLINE_INCLUDE_DIR AND READLINE_LIBRARY)
17 | SET(READLINE_FOUND FALSE)
18 | MESSAGE(FATAL_ERROR "Readline library not found!")
19 | ENDIF (READLINE_INCLUDE_DIR AND READLINE_LIBRARY)
20 |
--------------------------------------------------------------------------------
/example/bcasttest.conf:
--------------------------------------------------------------------------------
1 | # This config will connect ardupilot sitl to a gcs such as qgroundcontrol
2 | [sitl]
3 | type=udp
4 | localport=14550
5 |
6 | [other]
7 | type=udp
8 | targetport=14559
9 |
10 | [bcast]
11 | type=udpbcast
12 | bcastlock=false
13 | bcastip=192.168.0.255
14 | bcastport=14553
15 |
--------------------------------------------------------------------------------
/example/sitl2gcs.conf:
--------------------------------------------------------------------------------
1 | # This config will connect ardupilot sitl to a gcs such as qgroundcontrol
2 | [sitl]
3 | type=udp
4 | localport=14550
5 |
6 | [gcs]
7 | type=udp
8 | targetport=14555
9 |
--------------------------------------------------------------------------------
/example/test.conf:
--------------------------------------------------------------------------------
1 | #[aseriallink]
2 | # type=serial
3 | # port=/dev/ttyUSB0
4 | # baud=57600
5 |
6 | [audplink]
7 | type=udp
8 | targetip=127.0.0.1
9 | targetport=14559
10 | localport=14550
11 | sim_enable=true
12 | sim_packet_loss=25
13 | packet_drop_enable=true
14 |
15 | [audplink2]
16 | type=udp
17 | targetip=127.0.0.1
18 | targetport=14555
19 | localport=14556
--------------------------------------------------------------------------------
/src/asyncsocket.cpp:
--------------------------------------------------------------------------------
1 | /* CMAVNode
2 | * Monash UAS
3 | *
4 | * SOCKET CLASS
5 | * This class extends 'link' and overrides it methods to handle socket communications
6 | */
7 |
8 | #include "asyncsocket.h"
9 |
10 | // Fully defined constructor
11 | asyncsocket::asyncsocket(
12 | const std::string& host,
13 | const std::string& hostport,
14 | const std::string& listenport,
15 | link_info info_) : io_service_(), mlink(info_),
16 | socket_(io_service_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), std::stoi(listenport)))
17 | {
18 | prep(host, hostport);
19 | }
20 |
21 | // Client constructor
22 | asyncsocket::asyncsocket(
23 | const std::string& host,
24 | const std::string& hostport,
25 | link_info info_) : io_service_(), mlink(info_),
26 | socket_(io_service_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0))
27 | {
28 | prep(host, hostport);
29 | }
30 |
31 | // helper function for client constructors
32 | void asyncsocket::prep(
33 | const std::string& host,
34 | const std::string& hostport
35 | )
36 | {
37 | try
38 | {
39 | boost::asio::ip::address addr = boost::asio::ip::address::from_string(host);
40 | endpoint_.address(addr);
41 | endpoint_.port(std::stoi(hostport));
42 | }
43 | catch(std::exception e)
44 | {
45 | // probably not supplied an IP address; try resolving it:
46 | boost::asio::ip::udp::resolver resolver(io_service_);
47 | boost::asio::ip::udp::resolver::query query(boost::asio::ip::udp::v4(), host, hostport);
48 | boost::asio::ip::udp::resolver::iterator iter = resolver.resolve(query);
49 | endpoint_ = *iter;
50 | }
51 |
52 | //Start the read and write threads
53 | write_thread = boost::thread(&asyncsocket::runWriteThread, this);
54 |
55 | //Start the receive
56 | receive();
57 |
58 | read_thread = boost::thread(&asyncsocket::runReadThread, this);
59 | }
60 |
61 | // Server constructor
62 | asyncsocket::asyncsocket(
63 | const std::string& listenport,
64 | link_info info_) : io_service_(), mlink(info_),
65 | socket_(io_service_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), std::stoi(listenport)))
66 | {
67 | //Start the read and write threads
68 | write_thread = boost::thread(&asyncsocket::runWriteThread, this);
69 |
70 | //Start the receive
71 | receive();
72 |
73 | read_thread = boost::thread(&asyncsocket::runReadThread, this);
74 | }
75 |
76 | // Broadcast constructor
77 | asyncsocket::asyncsocket(bool bcastlock,
78 | const std::string& bindaddress,
79 | const std::string& bcastaddress,
80 | const std::string& bcastport,
81 | link_info info_) : io_service_(), mlink(info_),
82 | socket_(io_service_, boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(bindaddress), 0))
83 | {
84 | socket_.set_option(boost::asio::ip::udp::socket::reuse_address(true));
85 | socket_.set_option(boost::asio::socket_base::broadcast(true));
86 |
87 | boost::asio::ip::udp::endpoint senderEndpoint(boost::asio::ip::address_v4::from_string(bcastaddress), std::stoi(bcastport));
88 | endpoint_ = senderEndpoint;
89 |
90 | //Start the read and write threads
91 | write_thread = boost::thread(&asyncsocket::runWriteThread, this);
92 |
93 | endpointlock = bcastlock;
94 | //Start the receive
95 | receive();
96 |
97 | read_thread = boost::thread(&asyncsocket::runReadThread, this);
98 | }
99 |
100 | asyncsocket::~asyncsocket()
101 | {
102 | //Force run() to return then join thread
103 | io_service_.stop();
104 | read_thread.join();
105 |
106 | //force write thread to return then join thread
107 | exitFlag = true;
108 | write_thread.join();
109 |
110 | //Debind
111 | socket_.close();
112 | }
113 |
114 | void asyncsocket::send(uint8_t *buf, std::size_t buf_size)
115 | {
116 | socket_.async_send_to(
117 | boost::asio::buffer(buf, buf_size), endpoint_,
118 | boost::bind(&asyncsocket::handleSendTo, this,
119 | boost::asio::placeholders::error,
120 | boost::asio::placeholders::bytes_transferred));
121 | }
122 |
123 | void asyncsocket::receive()
124 | {
125 | // async_receive_from will override endpoint_ so if we want to receive from multiple clients use async_receive
126 | auto bound = boost::bind(&asyncsocket::handleReceiveFrom, this,
127 | boost::asio::placeholders::error,
128 | boost::asio::placeholders::bytes_transferred);
129 | auto buffer = boost::asio::buffer(data_in_, MAV_INCOMING_BUFFER_LENGTH);
130 | if(!endpointlock)
131 | {
132 | //this one only gets used for broadcast when we want to support multiple clients
133 | socket_.async_receive(buffer, bound);
134 | }
135 | else
136 | {
137 | socket_.async_receive_from(buffer, endpoint_, bound);
138 | if (sender_endpoint_ == nullptr)
139 | {
140 | sender_endpoint_ = new boost::asio::ip::udp::endpoint();
141 | }
142 | (*sender_endpoint_) = endpoint_;
143 | }
144 | }
145 |
146 | void asyncsocket::processAndSend(mavlink_message_t *msgToConvert)
147 | {
148 | //pack into buf and get size_t
149 | uint8_t tmplen = mavlink_msg_to_send_buffer(data_out_, msgToConvert);
150 | //ERROR HANDLING?
151 |
152 | bool should_drop = shouldDropPacket();
153 | //send on socket
154 | if(!should_drop)
155 | send(data_out_, tmplen);
156 | }
157 |
158 |
159 |
160 | //Async callback receiver
161 | void asyncsocket::handleReceiveFrom(const boost::system::error_code& error,
162 | size_t bytes_recvd)
163 | {
164 | if (!error && bytes_recvd > 0)
165 | {
166 | //message received
167 | //do something
168 | mavlink_message_t msg;
169 | mavlink_status_t status;
170 |
171 | for (size_t i = 0; i < bytes_recvd; i++)
172 | {
173 | if (mavlink_parse_char(MAVLINK_COMM_0, data_in_[i], &msg, &status))
174 | {
175 | onMessageRecv(&msg);
176 | }
177 | }
178 |
179 | //And start reading again
180 | receive();
181 | }
182 | else
183 | {
184 | //nothing received or there was an error
185 | throw Exception("UDPClient: Error in handle_receive_from");
186 | }
187 | }
188 |
189 | //Async post send callback
190 | void asyncsocket::handleSendTo(const boost::system::error_code& error,
191 | size_t bytes_recvd)
192 | {
193 | if (!error && bytes_recvd > 0)
194 | {
195 | //Everything was ok
196 | }
197 | else
198 | {
199 | //There was an error
200 | }
201 | }
202 |
203 | void asyncsocket::runReadThread()
204 | {
205 | //gets run in thread
206 | //Because io_service.run() will block while socket is open
207 | io_service_.run();
208 | }
209 |
210 | void asyncsocket::runWriteThread()
211 | {
212 | //block so we dont send before the socket is initialized
213 | boost::this_thread::sleep(boost::posix_time::milliseconds(50));
214 | //busy wait on the spsc_queueo
215 | mavlink_message_t tmpMsg;
216 |
217 | // Thread loop
218 | while (!exitFlag)
219 | {
220 | while (qMavOut.pop(tmpMsg))
221 | {
222 | out_counter.decrement();
223 | processAndSend(&tmpMsg);
224 | }
225 | boost::this_thread::sleep(boost::posix_time::milliseconds(OUT_QUEUE_EMPTY_SLEEP));
226 | }
227 | }
228 |
--------------------------------------------------------------------------------
/src/asyncsocket.h:
--------------------------------------------------------------------------------
1 | /* CMAVNode
2 | * Monash UAS
3 | *
4 | * SOCKET CLASS
5 | * This class extends 'link' and overrides it methods to handle socket communications
6 | */
7 | #ifndef ASYNCSOCKET_H
8 | #define ASYNCSOCKET_H
9 |
10 | #include
11 | #include
12 | #include
13 | #include
14 |
15 | #include "mlink.h"
16 |
17 | class asyncsocket: public mlink
18 | {
19 | public:
20 | //Construct specifying all
21 | asyncsocket(
22 | const std::string& host,
23 | const std::string& hostport,
24 | const std::string& listenport,
25 | link_info info_);
26 |
27 | //Specify only receive
28 | asyncsocket(
29 | const std::string& listenport,
30 | link_info info_);
31 |
32 | //bcast
33 | asyncsocket(bool bcastlock,
34 | const std::string& bindaddress,
35 | const std::string& bcastaddress,
36 | const std::string& bcastport,
37 | link_info info_);
38 |
39 | //Specify only target
40 | asyncsocket(
41 | const std::string& host,
42 | const std::string& hostport,
43 | link_info info_);
44 |
45 | ~asyncsocket();
46 |
47 | //override virtuals from link
48 | void runWriteThread();
49 | void runReadThread();
50 |
51 | // return endpoint corresponding to sender (if any)
52 | boost::asio::ip::udp::endpoint *sender_endpoint() override
53 | {
54 | return sender_endpoint_;
55 | }
56 |
57 | private:
58 | //Callbacks for async send/recv
59 | void handleReceiveFrom(const boost::system::error_code& error,
60 | size_t bytes_recvd);
61 | void handleSendTo(const boost::system::error_code& error,
62 | size_t bytes_recvd);
63 |
64 | //UDP Stuff
65 | boost::asio::io_service io_service_;
66 | boost::asio::ip::udp::socket socket_;
67 | boost::asio::ip::udp::endpoint endpoint_;
68 |
69 | boost::asio::ip::udp::endpoint *sender_endpoint_;
70 |
71 | bool endpointlock = true;
72 |
73 | //takes message, puts onto buff and calls send
74 | void processAndSend(mavlink_message_t *msgToConvert);
75 |
76 | //Actually sends
77 | void send(uint8_t *buf, std::size_t buf_size);
78 | void receive(); //Starts a async receive
79 |
80 | void prep(const std::string& host, const std::string& hostport);
81 | };
82 |
83 | #endif
84 |
--------------------------------------------------------------------------------
/src/configfile.cpp:
--------------------------------------------------------------------------------
1 | #include "configfile.h"
2 |
3 | #include
4 | #include "../include/mavlink2/mavlink_get_info.h"
5 |
6 | int readConfigFile(std::string &filename, std::vector > &links)
7 | {
8 | ConfigFile _configFile = ConfigFile(filename);
9 |
10 | std::vector sections = _configFile.GetSections();
11 | std::cout << "Found " << sections.size() << " links" << std::endl;
12 |
13 | for (uint i = 0; i < sections.size(); i++)
14 | {
15 | std::string thisSection = sections.at(i);
16 | std::string type;
17 | bool isSerial = false;
18 | UDP_type udp_type_ = UDP_TYPE_NONE;
19 | if(!_configFile.strValue(thisSection, "type", &type))
20 | {
21 | std::cerr << "Link has no type - skipping" << std::endl;
22 | continue;
23 | }
24 | std::string serialport;
25 | int baud;
26 | std::string targetip;
27 | std::string bindip;
28 | std::string bcastip;
29 | bool flowcontrol = false;
30 | bool bcastlock = true;
31 | int targetport = 0;
32 | int localport = 0;
33 | int bcastport = 0;
34 |
35 | if( type.compare("serial") == 0)
36 | {
37 | if(!_configFile.strValue(thisSection, "port",&serialport) || !_configFile.intValue(thisSection, "baud", &baud))
38 | {
39 | std::cerr << "Link: " << thisSection << " is specified as serial but does not have valid port and baud" << std::endl;
40 | continue;
41 | }
42 |
43 | //Try to get flow control
44 | _configFile.boolValue(thisSection, "flow_control", &flowcontrol);
45 | isSerial = true;
46 | std::cout << "Valid Serial Link: " << thisSection << " Found at: " << serialport << ", baud: " << baud << std::endl;
47 | }
48 | else if(type.compare("udp") == 0 || type.compare("socket") == 0 || type.compare("udpbcast") == 0)
49 | {
50 | if(type.compare("udpbcast") == 0 && _configFile.intValue(thisSection, "bcastport", &bcastport) && _configFile.strValue(thisSection, "bcastip", &bcastip))
51 | {
52 | if(!_configFile.strValue(thisSection, "bindip", &bindip))
53 | {
54 | // setting bindip in config file specifies interface to broadcast on
55 | bindip = "0.0.0.0"; //If bind ip not specified use 0.0.0.0... ipv4_any()
56 | }
57 |
58 | if(bcastip.find("255") == std::string::npos)
59 | {
60 | std::cerr << "Link: " << thisSection << " does not have a valid broadcast address" << std::endl;
61 | continue;
62 | }
63 | _configFile.boolValue(thisSection, "bcastlock", &bcastlock);
64 | udp_type_ = UDP_TYPE_BROADCAST;
65 | }
66 | else if(_configFile.strValue(thisSection, "targetip", &targetip)
67 | && _configFile.intValue(thisSection, "targetport", &targetport)
68 | && _configFile.intValue(thisSection, "localport", &localport))
69 | {
70 | udp_type_ = UDP_TYPE_FULLY_SPECIFIED;
71 | }
72 | else if(_configFile.intValue(thisSection, "localport", &localport))
73 | {
74 | udp_type_ = UDP_TYPE_SERVER;
75 | }
76 | else if(_configFile.strValue(thisSection, "targetip", &targetip)
77 | && _configFile.intValue(thisSection, "targetport", &targetport))
78 | {
79 | udp_type_ = UDP_TYPE_CLIENT;
80 | }
81 | else if(_configFile.intValue(thisSection, "targetport", &targetport))
82 | {
83 | targetip = "localhost";
84 | udp_type_ = UDP_TYPE_CLIENT;
85 | }
86 | else
87 | {
88 | std::cerr << "Link: " << thisSection << " is specified as " << type << " but does not have valid ip and port" << std::endl;
89 | continue;
90 | }
91 | if(udp_type_ != UDP_TYPE_BROADCAST)
92 | {
93 | std::cout << "Valid UDP Link: " << thisSection << " Found at " << targetip << ":" << targetport << " -> " << localport << std::endl;
94 | }
95 | else
96 | {
97 | std::cout << "Valid UDPBroadcast Link: " << thisSection << " Found, broadcasting on port " << bcastport << " bound to: " << bindip << std::endl;
98 | }
99 | }
100 | else
101 | {
102 | std::cerr << "Link: " << thisSection << " has invalid link type: " << type << std::endl;
103 | continue;
104 | }
105 |
106 | link_info _info;
107 | readLinkInfo(&_configFile, thisSection, &_info);
108 | //if we made it this far without break we have a valid link of some sort
109 | if(isSerial)
110 | {
111 | links.push_back(std::shared_ptr(new serial(serialport
112 | ,std::to_string(baud)
113 | ,flowcontrol
114 | ,_info)));
115 | }
116 | else if (udp_type_ != UDP_TYPE_NONE)
117 | {
118 | switch(udp_type_)
119 | {
120 | case UDP_TYPE_FULLY_SPECIFIED:
121 | links.push_back(
122 | std::shared_ptr(new asyncsocket(targetip,
123 | std::to_string(targetport)
124 | ,std::to_string(localport)
125 | ,_info)));
126 | break;
127 | case UDP_TYPE_SERVER:
128 | links.push_back(
129 | std::shared_ptr(new asyncsocket(std::to_string(localport)
130 | ,_info)));
131 | break;
132 | case UDP_TYPE_CLIENT:
133 | links.push_back(
134 | std::shared_ptr(new asyncsocket(targetip,
135 | std::to_string(targetport)
136 | ,_info)));
137 | break;
138 | case UDP_TYPE_BROADCAST:
139 | links.push_back(
140 | std::shared_ptr(new asyncsocket(bcastlock,
141 | bindip,
142 | bcastip,
143 | std::to_string(bcastport)
144 | ,_info)));
145 | break;
146 | }
147 | }
148 | }
149 | return 0;
150 | }
151 |
152 | void readLinkInfo(ConfigFile* _configFile, std::string thisSection, link_info* _info)
153 | {
154 | // Parse the optional parts of the config file which end up in mlink::link_info
155 | std::vector output_only_from;
156 | std::string output_list_tmp;
157 | if(_configFile->strValue(thisSection, "output_only_from",&output_list_tmp ))
158 | {
159 | std::vector< std::string> output_only_from_strings;
160 | boost::split(output_only_from_strings, output_list_tmp, boost::is_any_of(","));
161 | for(unsigned int i = 0; i < output_only_from_strings.size(); i++)
162 | {
163 | //this is not typesafe need to fix
164 | output_only_from.push_back( atoi( output_only_from_strings.at(i).c_str() ) );
165 | }
166 | }
167 | else
168 | {
169 | output_only_from.push_back(0);
170 | }
171 |
172 | _info->link_name = thisSection;
173 | _info->output_only_from = output_only_from;
174 | _configFile->boolValue(thisSection, "sim_enable", &_info->sim_enable);
175 | if(_info->sim_enable)
176 | {
177 | //then sim_enable is true
178 | std::cout << "WARNING: Link has simulation options enabled" << std::endl;
179 | if(_configFile->intValue(thisSection, "sim_packet_loss", &_info->sim_packet_loss))
180 | {
181 | std::cout << "Packet loss set to " << _info->sim_packet_loss << "%" << std::endl;
182 | }
183 | }
184 |
185 | // Enable or disable packet dropping
186 | _configFile->boolValue(thisSection, "reject_repeat_packets", &_info->reject_repeat_packets);
187 |
188 | // Identify SiK radio links
189 | _configFile->boolValue(thisSection, "sik_radio", &_info->SiK_radio);
190 |
191 | // Enable sleep mode for the link
192 | _configFile->boolValue(thisSection, "sleep", &_info->sleep_enabled);
193 |
194 | //Message Filters
195 | std::string filter_string;
196 | if (_configFile->strValue(thisSection, "filter", &filter_string))
197 | {
198 | // Find the filter type separator
199 | size_t filter_type_separator = filter_string.find_first_of(':');
200 |
201 | // Filter type seporator has been found
202 | if (filter_type_separator != std::string::npos)
203 | {
204 | const std::unordered_map filter_type_map =
205 | {
206 | { "DROP", link_filter_type::DROP },
207 | { "ACCEPT", link_filter_type::ACCEPT },
208 | };
209 |
210 | // Extract the filter type string
211 | std::string filter_type_str = filter_string.substr(0, filter_type_separator);
212 |
213 | // Find the binding in the map
214 | auto filter_type_iter = filter_type_map.find(filter_type_str);
215 |
216 | // It's a valid filter type
217 | if (filter_type_iter != filter_type_map.end())
218 | {
219 | // Extract the filter messages string
220 | std::string filter_messages_str = filter_string.substr(filter_type_separator + 1);
221 |
222 | std::vector messages_strs;
223 |
224 | boost::split(messages_strs, filter_messages_str, boost::is_any_of(","));
225 |
226 | // Filter is not empty
227 | if (messages_strs[0].length())
228 | {
229 | _info->filter_type = filter_type_iter->second;
230 |
231 | const mavlink_message_info_t *message_info;
232 |
233 | // For every message name
234 | for (const std::string &filter_message_str : messages_strs) {
235 | // Find the MAVLink message information
236 | message_info = mavlink_get_message_info_by_name(filter_message_str.c_str());
237 |
238 | // Valid message name
239 | if (message_info)
240 | _info->filter_messages.insert(message_info->msgid);
241 | // Invalid message name
242 | else
243 | std::cout << "Failed to add message \"" << filter_message_str << "\" to the filter. Unknown message!"
244 | << std::endl;
245 | }
246 | }
247 | // Empty filter
248 | else
249 | std::cout << "Failed to load filter for \"" << _info->link_name << "\". No messages!" << std::endl;
250 | }
251 | // Unknown filter type
252 | else
253 | std::cout << "Failed to load filter for \"" << _info->link_name << "\". Uknown filter type \"" <<
254 | filter_type_str << "\"!" << std::endl;
255 | }
256 | // No filter message type
257 | else
258 | std::cout << "Failed to load filter for \"" << _info->link_name << "\". No filter type found!" << std::endl;
259 | }
260 | }
261 |
262 | std::string trim(std::string const& source, char const* delims = " \t\r\n")
263 | {
264 | std::string result(source);
265 | std::string::size_type index = result.find_last_not_of(delims);
266 | if(index != std::string::npos)
267 | result.erase(++index);
268 |
269 | index = result.find_first_not_of(delims);
270 | if(index != std::string::npos)
271 | result.erase(0, index);
272 | else
273 | result.erase();
274 | return result;
275 | }
276 |
277 | ConfigFile::ConfigFile()
278 | {
279 |
280 | }
281 |
282 | ConfigFile::ConfigFile(std::string const& configFile)
283 | {
284 | std::ifstream file(configFile.c_str());
285 |
286 | std::string line;
287 | std::string name;
288 | std::string value;
289 | std::string inSection;
290 | int posEqual;
291 | while (std::getline(file,line))
292 | {
293 |
294 | if (! line.length()) continue;
295 |
296 | if (line[0] == '#') continue;
297 | if (line[0] == ';') continue;
298 |
299 | if (line[0] == '[')
300 | {
301 | inSection=trim(line.substr(1,line.find(']')-1));
302 | sections_.push_back(inSection);
303 | continue;
304 | }
305 |
306 | posEqual=line.find('=');
307 | name = trim(line.substr(0,posEqual));
308 | value = trim(line.substr(posEqual+1));
309 |
310 | content_[inSection+'/'+name]=value;
311 | }
312 | }
313 |
314 | std::vector ConfigFile::GetSections()
315 | {
316 | return sections_;
317 | }
318 |
319 | bool ConfigFile::boolValue(std::string const& section, std::string const& entry, bool* value)
320 | {
321 | std::string str_value;
322 | if(!strValue(section, entry, &str_value)) return false;
323 |
324 | if(str_value.compare("true") == 0)
325 | {
326 | *value = true;
327 | return true;
328 | }
329 | else if(str_value.compare("false") == 0)
330 | {
331 | *value = false;
332 | return true;
333 | }
334 | else if(str_value.compare("1") == 0)
335 | {
336 | *value = true;
337 | return true;
338 | }
339 | else if(str_value.compare("0") == 0)
340 | {
341 | *value = false;
342 | return true;
343 | }
344 |
345 | return false;
346 |
347 | }
348 | bool ConfigFile::intValue(std::string const& section, std::string const& entry, int* value)
349 | {
350 | std::string str_value;
351 | if(!strValue(section, entry, &str_value)) return false;
352 |
353 | try
354 | {
355 | *value = std::stoi(str_value);
356 | }
357 | catch(std:: exception e)
358 | {
359 | //converting to int caused a problem
360 | return false;
361 | }
362 | return true;
363 | }
364 |
365 | bool ConfigFile::strValue(std::string const& section, std::string const& entry, std::string* value)
366 | {
367 | std::map::const_iterator ci = content_.find(section + '/' + entry);
368 |
369 | if (ci == content_.end()) return false; //then the requested value does not exist
370 |
371 | *value = ci->second;
372 | return true;
373 | }
374 |
--------------------------------------------------------------------------------
/src/configfile.h:
--------------------------------------------------------------------------------
1 | #ifndef CONFIG_FILE_H__
2 | #define CONFIG_FILE_H__
3 |
4 | #include
5 | #include