PX4 Firmware
PX4 Autopilot Software http://px4.io
uavcannode_main.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 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 #pragma once
35 
36 #include <px4_platform_common/px4_config.h>
38 #include <drivers/device/device.h>
39 #include <uavcan/protocol/global_time_sync_slave.hpp>
40 #include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
41 #include <uavcan/node/timer.hpp>
42 /**
43  * @file uavcan_main.hpp
44  *
45  * Defines basic functionality of UAVCAN node.
46  *
47  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
48  * David Sidrane <david_s5@nscdg.com>
49  */
50 
51 #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 1
52 #define UAVCAN_DEVICE_PATH "/dev/uavcan/node"
53 
54 // we add 1 to allow for busevent
55 #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+1)
56 
57 /**
58  * A UAVCAN node.
59  */
60 class UavcanNode : public cdev::CDev
61 {
62  /*
63  * This memory is reserved for uavcan to use as over flow for message
64  * Coming from multiple sources that my not be considered at development
65  * time.
66  *
67  * The call to getNumFreeBlocks will tell how many blocks there are
68  * free -and multiply it times getBlockSize to get the number of bytes
69  *
70  */
71  static constexpr unsigned MemPoolSize = 2048;
72 
73  /*
74  * This memory is reserved for uavcan to use for queuing CAN frames.
75  * At 1Mbit there is approximately one CAN frame every 200 uS.
76  * The number of buffers sets how long you can go without calling
77  * node_spin_xxxx. Since our task is the only one running and the
78  * driver will light the fd when there is a CAN frame we can nun with
79  * a minimum number of buffers to conserver memory. Each buffer is
80  * 32 bytes. So 5 buffers costs 160 bytes and gives us a maximum required
81  * poll rate of ~1 mS
82  *
83  */
84  static constexpr unsigned RxQueueLenPerIface = 5;
85 
86  /*
87  * This memory is uses for the tasks stack size
88  */
89 
90  static constexpr unsigned StackSize = 2500;
91 
92 public:
93  typedef uavcan::Node<MemPoolSize> Node;
95  typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate;
96 
97  UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
98 
99  virtual ~UavcanNode();
100 
101  virtual int ioctl(file *filp, int cmd, unsigned long arg);
102 
103  static int start(uavcan::NodeID node_id, uint32_t bitrate);
104 
105  Node &get_node() { return _node; }
106 
107  int teardown();
108 
109  void print_info();
110 
111  static UavcanNode *instance() { return _instance; }
112 
113 
114  /* The bit rate that can be passed back to the bootloader */
115 
116  int32_t active_bitrate;
117 
118 
119 private:
120  void fill_node_info();
121  int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events);
122  void node_spin_once();
123  int run();
124  static void busevent_signal_trampoline();
125 
126 
127  px4_sem_t _sem; ///< semaphore for scheduling the task
128  int _task = -1; ///< handle to the OS task
129  bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
130 
131  static UavcanNode *_instance; ///< singleton pointer
132  Node _node; ///< library instance
133  pthread_mutex_t _node_mutex;
134  uavcan::GlobalTimeSyncSlave _time_sync_slave;
135 
136  typedef uavcan::MethodBinder<UavcanNode *,
137  void (UavcanNode::*)(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &,
138  uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &)>
140 
141  uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateCallBack> _fw_update_listner;
142  void cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &req,
143  uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp);
144 
145 public:
146 
147  /* A timer used to reboot after the response is sent */
148 
149  uavcan::TimerEventForwarder<void (*)(const uavcan::TimerEvent &)> _reset_timer;
150 
151 };
virtual ~UavcanNode()
static int start(uavcan::NodeID node_id, uint32_t bitrate)
uavcan_stm32::CanInitHelper< RxQueueLenPerIface > CanInitHelper
uavcan::Node _node
library instance
static constexpr unsigned MemPoolSize
static void busevent_signal_trampoline()
Helper class.
Definition: can.hpp:291
pthread_mutex_t _node_mutex
virtual int init()
Definition: CDev.cpp:119
uavcan::Node< MemPoolSize > Node
void print_info()
Node & get_node()
uavcan::ServiceServer< BeginFirmwareUpdate, BeginFirmwareUpdateCallBack > _fw_update_listner
uavcan::TimerEventForwarder< void(*)(const uavcan::TimerEvent &)> _reset_timer
px4::atomic_bool _task_should_exit
flag to indicate to tear down the CAN driver
int32_t active_bitrate
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock)
Definition: uavcan_main.cpp:78
A UAVCAN node.
Abstract class for any character device.
Definition: CDev.hpp:58
px4_sem_t _sem
semaphore for scheduling the task
void cb_beginfirmware_update(const uavcan::ReceivedDataStructure< UavcanNode::BeginFirmwareUpdate::Request > &req, uavcan::ServiceResponseDataStructure< UavcanNode::BeginFirmwareUpdate::Response > &rsp)
static constexpr unsigned StackSize
Node _node
library instance
static UavcanNode * _instance
singleton pointer
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ReceivedDataStructure< UavcanNode::BeginFirmwareUpdate::Request > &, uavcan::ServiceResponseDataStructure< UavcanNode::BeginFirmwareUpdate::Response > &)> BeginFirmwareUpdateCallBack
static UavcanNode * instance()
int teardown()
struct @83::@85::@87 file
uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate
uavcan::GlobalTimeSyncSlave _time_sync_slave
static constexpr unsigned RxQueueLenPerIface
int _task
handle to the OS task
virtual int ioctl(file *filp, int cmd, unsigned long arg)
void node_spin_once()
void fill_node_info()