PX4 Firmware
PX4 Autopilot Software http://px4.io
gnss.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2014-2019 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 gnss.hpp
36  *
37  * UAVCAN <--> ORB bridge for GNSS messages:
38  * uavcan.equipment.gnss.Fix (deprecated, but still supported for backward compatibility)
39  * uavcan.equipment.gnss.Fix2
40  *
41  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
42  * @author Andrew Chambers <achamber@gmail.com>
43  */
44 
45 #pragma once
46 
47 #include <uORB/Subscription.hpp>
50 
51 #include <uavcan/uavcan.hpp>
52 #include <uavcan/equipment/gnss/Auxiliary.hpp>
53 #include <uavcan/equipment/gnss/Fix.hpp>
54 #include <uavcan/equipment/gnss/Fix2.hpp>
55 
56 #include "sensor_bridge.hpp"
57 
59 {
60  static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10;
61 
62 public:
63  static const char *const NAME;
64 
65  UavcanGnssBridge(uavcan::INode &node);
66  ~UavcanGnssBridge() = default;
67 
68  const char *get_name() const override { return NAME; }
69 
70  int init() override;
71 
72  unsigned get_num_redundant_channels() const override;
73 
74  void print_status() const override;
75 
76 private:
77  /**
78  * GNSS fix message will be reported via this callback.
79  */
80  void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
81  void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
82  void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
83 
84  template <typename FixType>
85  void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
86  const float (&pos_cov)[9], const float (&vel_cov)[9],
87  const bool valid_pos_cov, const bool valid_vel_cov);
88 
89  void broadcast_from_orb(const uavcan::TimerEvent &);
90 
91  typedef uavcan::MethodBinder < UavcanGnssBridge *,
92  void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
94 
95  typedef uavcan::MethodBinder < UavcanGnssBridge *,
96  void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
98 
99  typedef uavcan::MethodBinder < UavcanGnssBridge *,
100  void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &) >
102 
103  typedef uavcan::MethodBinder<UavcanGnssBridge *,
104  void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)>
106 
107  uavcan::INode &_node;
108 
109  uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
110  uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
111  uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
112 
113  uavcan::Publisher<uavcan::equipment::gnss::Fix2> _pub_fix2;
114 
115  uavcan::TimerEventForwarder<TimerCbBinder> _orb_to_uavcan_pub_timer;
116 
120 
122  uORB::Subscription _orb_sub_gnss{ORB_ID(vehicle_gps_position)};
123 
126  bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data?
127 
128 };
void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure< uavcan::equipment::gnss::Fix2 > &msg)
Definition: gnss.cpp:145
int init() override
Starts the bridge.
Definition: gnss.cpp:65
~UavcanGnssBridge()=default
bool _old_fix_subscriber_active
Definition: gnss.hpp:125
uavcan::MethodBinder< UavcanGnssBridge *, void(UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure< uavcan::equipment::gnss::Auxiliary > &) > AuxiliaryCbBinder
Definition: gnss.hpp:93
uavcan::Publisher< uavcan::equipment::gnss::Fix2 > _pub_fix2
Definition: gnss.hpp:113
bool _system_clock_set
Have we set the system clock at least once from GNSS data?
Definition: gnss.hpp:126
static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ
Definition: gnss.hpp:60
int _receiver_node_id
Definition: gnss.hpp:124
UavcanGnssBridge(uavcan::INode &node)
Definition: gnss.cpp:54
const char * get_name() const override
Returns ASCII name of the bridge.
Definition: gnss.hpp:68
uavcan::INode & _node
Definition: gnss.hpp:107
uORB::Subscription _orb_sub_gnss
Definition: gnss.hpp:122
uavcan::TimerEventForwarder< TimerCbBinder > _orb_to_uavcan_pub_timer
Definition: gnss.hpp:115
void broadcast_from_orb(const uavcan::TimerEvent &)
Definition: gnss.cpp:414
uavcan::MethodBinder< UavcanGnssBridge *, void(UavcanGnssBridge::*)(const uavcan::TimerEvent &)> TimerCbBinder
Definition: gnss.hpp:105
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
uORB::PublicationMulti< vehicle_gps_position_s > _gps_pub
Definition: gnss.hpp:121
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure< uavcan::equipment::gnss::Fix > &msg)
Definition: gnss.cpp:130
uavcan::MethodBinder< UavcanGnssBridge *, void(UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure< uavcan::equipment::gnss::Fix > &) > FixCbBinder
Definition: gnss.hpp:97
uavcan::Subscriber< uavcan::equipment::gnss::Fix, FixCbBinder > _sub_fix
Definition: gnss.hpp:110
uavcan::MethodBinder< UavcanGnssBridge *, void(UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure< uavcan::equipment::gnss::Fix2 > &) > Fix2CbBinder
Definition: gnss.hpp:101
unsigned get_num_redundant_channels() const override
Returns number of active redundancy channels.
Definition: gnss.cpp:101
uavcan::Subscriber< uavcan::equipment::gnss::Fix2, Fix2CbBinder > _sub_fix2
Definition: gnss.hpp:111
void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure< uavcan::equipment::gnss::Auxiliary > &msg)
GNSS fix message will be reported via this callback.
Definition: gnss.cpp:121
uavcan::Subscriber< uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder > _sub_auxiliary
Definition: gnss.hpp:109
float _last_gnss_auxiliary_hdop
Definition: gnss.hpp:118
static const char *const NAME
Definition: gnss.hpp:63
float _last_gnss_auxiliary_vdop
Definition: gnss.hpp:119
uint64_t _last_gnss_auxiliary_timestamp
Definition: gnss.hpp:117
void print_status() const override
Prints current status in a human readable format to stdout.
Definition: gnss.cpp:107
void process_fixx(const uavcan::ReceivedDataStructure< FixType > &msg, const float(&pos_cov)[9], const float(&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov)
Definition: gnss.cpp:262
A sensor bridge class must implement this interface.