PX4 Firmware
PX4 Autopilot Software http://px4.io
ubx_sim.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file ubx.cpp
36  *
37  * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description
38  * including Prototol Specification.
39  *
40  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
41  * @author Julian Oes <joes@student.ethz.ch>
42  * @author Anton Babushkin <anton.babushkin@me.com>
43  *
44  * @author Hannes Delago
45  * (rework, add ubx7+ compatibility)
46  *
47  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
48  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
49  */
50 
51 #include <assert.h>
52 #include <math.h>
53 #include <poll.h>
54 #include <stdio.h>
55 #include <string.h>
56 #include <time.h>
57 #include <unistd.h>
58 
59 #include <systemlib/err.h>
60 #include <uORB/uORB.h>
63 #include <drivers/drv_hrt.h>
64 
65 #include "ubx_sim.h"
66 #include <simulator/simulator.h>
67 
68 #define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
69 #define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
70 #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
71 #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
72 
73 
74 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
75  _fd(fd),
76  _gps_position(gps_position),
77  _satellite_info(satellite_info),
78 {
79 }
80 
81 UBX::~UBX()
82 {
83 }
84 
86 {
87  return 0;
88 }
89 
90 int // -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
91 UBX_SIM::receive(const unsigned timeout)
92 {
93  /* copy data from simulator here */
94  usleep(1000000);
95  return 1;
96 }
API for the uORB lightweight object broker.
This module interfaces via MAVLink to a software in the loop simulator (SITL) such as jMAVSim or Gaze...
High-resolution timer with callouts and timekeeping.
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
int fd
Definition: dataman.cpp:146
int configure(unsigned &baudrate)
Definition: ubx_sim.cpp:85
int receive(const unsigned timeout)
Definition: ubx_sim.cpp:91