PX4 Firmware
PX4 Autopilot Software http://px4.io
uavcan_main.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2014 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 uavcan_main.hpp
36  *
37  * Defines basic functinality of UAVCAN node.
38  *
39  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
40  * Andreas Jochum <Andreas@NicaDrone.com>
41  */
42 
43 #pragma once
44 
45 #include <px4_platform_common/px4_config.h>
46 #include <px4_platform_common/atomic.h>
47 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
48 
49 #include "uavcan_driver.hpp"
50 #include "uavcan_servers.hpp"
51 #include "allocator.hpp"
52 #include "actuators/esc.hpp"
53 #include "actuators/hardpoint.hpp"
55 
56 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
57 #include <uavcan/protocol/global_time_sync_master.hpp>
58 #include <uavcan/protocol/global_time_sync_slave.hpp>
59 #include <uavcan/protocol/node_status_monitor.hpp>
60 #include <uavcan/protocol/param/GetSet.hpp>
61 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
62 #include <uavcan/protocol/RestartNode.hpp>
63 
66 #include <lib/perf/perf_counter.h>
67 
68 #include <uORB/Subscription.hpp>
70 
71 class UavcanNode;
72 
73 /**
74  * UAVCAN mixing class.
75  * It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling
76  * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at
77  * a fixed rate or upon bus updates).
78  * Both work items are expected to run on the same work queue.
79  */
81 {
82 public:
83  UavcanMixingInterface(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
84  : OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
85  _node_mutex(node_mutex),
86  _esc_controller(esc_controller) {}
87 
88  bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
89  unsigned num_outputs, unsigned num_control_groups_updated) override;
90 
91  void mixerChanged() override;
92 
94 
95 protected:
96  void Run() override;
97 private:
98  friend class UavcanNode;
99  pthread_mutex_t &_node_mutex;
102 };
103 
104 /**
105  * A UAVCAN node.
106  */
107 class UavcanNode : public cdev::CDev, public px4::ScheduledWorkItem, public ModuleParams
108 {
109  static constexpr unsigned MaxBitRatePerSec = 1000000;
110  static constexpr unsigned bitPerFrame = 148;
111  static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame;
112  static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1);
113 
114  static constexpr unsigned ScheduleIntervalMs = 3;
115 
116 
117  /*
118  * This memory is reserved for uavcan to use for queuing CAN frames.
119  * At 1Mbit there is approximately one CAN frame every 145 uS.
120  * The number of buffers sets how long you can go without calling
121  * node_spin_xxxx. Since our task is the only one running and the
122  * driver will light the callback when there is a CAN frame we can nun with
123  * a minimum number of buffers to conserver memory. Each buffer is
124  * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate
125  * of ~1 mS
126  * 1000000/200
127  */
128 
129  static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * ScheduleIntervalMs; // At
130 
131 public:
132  typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
133  enum eServerAction : int {None, Start, Stop, CheckFW, Busy};
134 
135  UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
136 
137  virtual ~UavcanNode();
138 
139  virtual int ioctl(file *filp, int cmd, unsigned long arg);
140 
141  static int start(uavcan::NodeID node_id, uint32_t bitrate);
142 
143  uavcan::Node<> &get_node() { return _node; }
144 
145  int teardown();
146 
147  void print_info();
148 
149  void shrink();
150 
151  void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
152 
153  static UavcanNode *instance() { return _instance; }
154  static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
155  int fw_server(eServerAction action);
156  void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
157  int list_params(int remote_node_id);
158  int save_params(int remote_node_id);
159  int set_param(int remote_node_id, const char *name, char *value);
160  int get_param(int remote_node_id, const char *name);
161  int reset_node(int remote_node_id);
162 
163  static void busevent_signal_trampoline();
164 
165 protected:
166  void Run() override;
167 private:
168  void fill_node_info();
169  int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
170  void node_spin_once();
171 
172  int start_fw_server();
173  int stop_fw_server();
174  int request_fw_check();
175 
176  int print_params(uavcan::protocol::param::GetSet::Response &resp);
177  int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
178  void update_params();
179 
180  void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; }
181  void free_setget_response(void) { _setget_response = nullptr; }
182 
183  void enable_idle_throttle_when_armed(bool value);
184 
185  px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
186  px4::atomic<int> _fw_server_action{None};
187  int _fw_server_status{-1};
188 
189  bool _is_armed{false}; ///< the arming status of the actuators on the bus
190 
191  unsigned _output_count{0}; ///< number of actuators currently available
192 
193  static UavcanNode *_instance; ///< singleton pointer
194 
196 
197  uavcan::Node<> _node; ///< library instance
198  pthread_mutex_t _node_mutex;
201  UavcanMixingInterface _mixing_interface{_node_mutex, _esc_controller};
203  uavcan::GlobalTimeSyncMaster _time_sync_master;
204  uavcan::GlobalTimeSyncSlave _time_sync_slave;
205  uavcan::NodeStatusMonitor _node_status_monitor;
206 
207  List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
208 
209  ITxQueueInjector *_tx_injector{nullptr};
210 
211  bool _idle_throttle_when_armed{false};
212  int32_t _idle_throttle_when_armed_param{0};
213 
214  uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
215 
218 
219  void handle_time_sync(const uavcan::TimerEvent &);
220 
221  typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
222  uavcan::TimerEventForwarder<TimerCallback> _master_timer;
223 
224  bool _callback_success{false};
225 
226  uavcan::protocol::param::GetSet::Response *_setget_response{nullptr};
227 
228  typedef uavcan::MethodBinder<UavcanNode *,
229  void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
230  typedef uavcan::MethodBinder<UavcanNode *,
231  void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
232  typedef uavcan::MethodBinder<UavcanNode *,
233  void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
234 
235  void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
236  void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
237  void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
238 
239 };
uavcan::TimerEventForwarder< TimerCallback > _master_timer
uavcan::Node _node
library instance
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::GetSet > &)> GetSetCallback
void Run() override
MixingOutput & mixingOutput()
Definition: uavcan_main.hpp:93
UAVCAN_DRIVER::CanInitHelper< RxQueueLenPerIface > CanInitHelper
friend class UavcanNode
Definition: uavcan_main.hpp:98
pthread_mutex_t _node_mutex
void attachITxQueueInjector(ITxQueueInjector *injector)
MixingOutput _mixing_output
UAVCAN <–> ORB bridge for ESC messages: uavcan.equipment.esc.RawCommand uavcan.equipment.esc.RPMCommand uavcan.equipment.esc.Status.
UavcanEscController & _esc_controller
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::RestartNode > &)> RestartNodeCallback
void mixerChanged() override
called whenever the mixer gets updated/reset
static constexpr int MAX_ACTUATORS
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
pthread_mutex_t & _node_mutex
Definition: uavcan_main.hpp:99
Definition: List.hpp:59
A UAVCAN node.
Abstract class for any character device.
Definition: CDev.hpp:58
UavcanEscController _esc_controller
Drive scheduling based on subscribed actuator controls topics (via uORB callbacks) ...
Header common to all counters.
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
void init()
Activates/configures the hardware registers.
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override
Callback to update the (physical) actuator outputs in the driver.
The UavcanHardpointController class.
Definition: hardpoint.hpp:51
perf_counter_t _interval_perf
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::ServiceCallResult< uavcan::protocol::param::ExecuteOpcode > &)> ExecuteOpcodeCallback
Base class for an output module.
UAVCAN mixing class.
Definition: uavcan_main.hpp:80
static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
Definition: vmount.cpp:525
static int start()
Definition: dataman.cpp:1452
const char * name
Definition: tests_main.c:58
List< IUavcanSensorBridge * > _sensor_bridges
List of active sensor bridges.
static UavcanNode * _instance
singleton pointer
px4_sem_t _server_command_sem
uavcan::NodeStatusMonitor _node_status_monitor
void free_setget_response(void)
static UavcanNode * instance()
struct @83::@85::@87 file
uavcan::GlobalTimeSyncSlave _time_sync_slave
Definition: bst.cpp:62
UavcanMixingInterface(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
Definition: uavcan_main.hpp:83
uavcan::MethodBinder< UavcanNode *, void(UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback
Defines basic functionality of UAVCAN node.
uavcan::GlobalTimeSyncMaster _time_sync_master
uavcan_node::Allocator _pool_allocator
This handles the mixing, arming/disarming and all subscriptions required for that.
perf_counter_t _cycle_perf
uavcan::Node & get_node()
UavcanHardpointController _hardpoint_controller
Performance measuring tools.
This interface defines one method that will be called by the main node thread periodically in order t...