1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
|
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/device/device.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/actuator_direct.h>
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
/**
* @file uavcan_main.hpp
*
* Defines basic functinality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
/**
* A UAVCAN node.
*/
class UavcanNode : public device::CDev
{
static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why
static constexpr unsigned RxQueueLenPerIface = 64;
static constexpr unsigned StackSize = 3000;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
virtual ~UavcanNode();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node &get_node() { return _node; }
// TODO: move the actuator mixing stuff into the ESC controller class
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
int teardown();
int arm_actuators(bool arm);
int leds(int r, int g, int b, int hz = 2);
void print_info();
static UavcanNode *instance() { return _instance; }
private:
void fill_node_info();
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
int _armed_sub = -1; ///< uORB subscription of the arming status
actuator_armed_s _armed = {}; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
test_motor_s _test_motor = {};
bool _test_in_progress = false;
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
UavcanEscController _esc_controller;
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
unsigned _poll_fds_num = 0;
int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic
uint8_t _actuator_direct_poll_fd_num = 0;
actuator_direct_s _actuator_direct = {};
actuator_outputs_s _outputs = {};
// index into _poll_fds for each _control_subs handle
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
};
|
