PX4 Firmware
PX4 Autopilot Software http://px4.io
esc.cpp
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 esc.cpp
36  *
37  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
38  */
39 
40 #include "esc.hpp"
41 #include <systemlib/err.h>
42 #include <drivers/drv_hrt.h>
43 
44 #define MOTOR_BIT(x) (1<<(x))
45 
46 using namespace time_literals;
47 
49  _node(node),
50  _uavcan_pub_raw_cmd(node),
51  _uavcan_sub_status(node),
52  _orb_timer(node)
53 {
55 }
56 
58 {
59 }
60 
61 int
63 {
64  // ESC status subscription
66 
67  if (res < 0) {
68  PX4_ERR("ESC status sub failed %i", res);
69  return res;
70  }
71 
72  // ESC status will be relayed from UAVCAN bus into ORB at this rate
74  _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
75 
76  return res;
77 }
78 
79 void
80 UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
81 {
82  if (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) {
83  num_outputs = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
84  }
85 
86  if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) {
87  num_outputs = esc_status_s::CONNECTED_ESC_MAX;
88  }
89 
90  /*
91  * Rate limiting - we don't want to congest the bus
92  */
93  const auto timestamp = _node.getMonotonicTime();
94 
95  if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
96  return;
97  }
98 
99  _prev_cmd_pub = timestamp;
100 
101  /*
102  * Fill the command message
103  * If unarmed, we publish an empty message anyway
104  */
105  uavcan::equipment::esc::RawCommand msg;
106 
107  for (unsigned i = 0; i < num_outputs; i++) {
108  if (stop_motors || outputs[i] == DISARMED_OUTPUT_VALUE) {
109  msg.cmd.push_back(static_cast<unsigned>(0));
110 
111  } else {
112  msg.cmd.push_back(static_cast<int>(outputs[i]));
113  }
114  }
115 
116  /*
117  * Remove channels that are always zero.
118  * The objective of this optimization is to avoid broadcasting multi-frame transfers when a single frame
119  * transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all
120  * non-specified ESC setpoints should be considered zero.
121  * The positive outcome is a (marginally) lower bus traffic and lower CPU load.
122  *
123  * From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why
124  * the mixer returns more outputs than are actually used.
125  */
126  for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) {
127  if (msg.cmd[index] != 0) {
128  _max_number_of_nonzero_outputs = index + 1;
129  break;
130  }
131  }
132 
133  msg.cmd.resize(_max_number_of_nonzero_outputs);
134 
135  /*
136  * Publish the command message to the bus
137  * Note that for a quadrotor it takes one CAN frame
138  */
139  _uavcan_pub_raw_cmd.broadcast(msg);
140 }
141 
142 void
143 UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
144 {
145  if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
146  auto &ref = _esc_status.esc[msg.esc_index];
147 
148  ref.esc_address = msg.getSrcNodeID().get();
149  ref.timestamp = hrt_absolute_time();
150  ref.esc_voltage = msg.voltage;
151  ref.esc_current = msg.current;
152  ref.esc_temperature = msg.temperature;
153  ref.esc_rpm = msg.rpm;
154  ref.esc_errorcount = msg.error_count;
155  }
156 }
157 
158 void
159 UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
160 {
163  _esc_status.counter += 1;
164  _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
166 
168 }
169 
170 uint8_t
172 {
173  int esc_status_flags = 0;
174  const hrt_abstime now = hrt_absolute_time();
175 
176  for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
177 
178  if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) {
179  esc_status_flags |= (1 << index);
180  }
181 
182  }
183 
184  return esc_status_flags;
185 }
uavcan::MonotonicTime _prev_cmd_pub
rate limiting
Definition: esc.hpp:112
struct esc_report_s esc[8]
Definition: esc_status.h:67
uint8_t esc_online_flags
Definition: esc_status.h:65
uavcan::INode & _node
Definition: esc.hpp:113
uavcan::MethodBinder< UavcanEscController *, void(UavcanEscController::*)(const uavcan::ReceivedDataStructure< uavcan::equipment::esc::Status > &)> StatusCbBinder
Definition: esc.hpp:98
esc_status_s _esc_status
Definition: esc.hpp:103
UavcanEscController(uavcan::INode &node)
Definition: esc.cpp:48
uint8_t esc_connectiontype
Definition: esc_status.h:64
uint8_t _rotor_count
Definition: esc.hpp:107
uint64_t timestamp
Definition: esc_status.h:61
UAVCAN <–> ORB bridge for ESC messages: uavcan.equipment.esc.RawCommand uavcan.equipment.esc.RPMCommand uavcan.equipment.esc.Status.
uORB::PublicationMulti< esc_status_s > _esc_status_pub
Definition: esc.hpp:105
uavcan::Publisher< uavcan::equipment::esc::RawCommand > _uavcan_pub_raw_cmd
Definition: esc.hpp:114
High-resolution timer with callouts and timekeeping.
static constexpr int MAX_ACTUATORS
Definition: esc.hpp:60
uint8_t check_escs_status()
Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offli...
Definition: esc.cpp:171
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
void esc_status_sub_cb(const uavcan::ReceivedDataStructure< uavcan::equipment::esc::Status > &msg)
ESC status message reception will be reported via this callback.
Definition: esc.cpp:143
uint64_t timestamp
Definition: esc_report.h:53
static constexpr unsigned MAX_RATE_HZ
XXX make this configurable.
Definition: esc.hpp:61
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
uavcan::TimerEventForwarder< TimerCbBinder > _orb_timer
Definition: esc.hpp:116
uint16_t counter
Definition: esc_status.h:62
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
uint8_t esc_count
Definition: esc_status.h:63
uavcan::Subscriber< uavcan::equipment::esc::Status, StatusCbBinder > _uavcan_sub_status
Definition: esc.hpp:115
static constexpr uint16_t DISARMED_OUTPUT_VALUE
Definition: esc.hpp:62
uint8_t esc_address
Definition: esc_report.h:59
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
Definition: esc.cpp:80
static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY
0..31, inclusive, 0 - highest, 31 - lowest
Definition: esc.hpp:95
void orb_pub_timer_cb(const uavcan::TimerEvent &event)
ESC status will be published to ORB from this callback (fixed rate).
Definition: esc.cpp:159
bool publish(const T &data)
Publish the struct.
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ
Definition: esc.hpp:94
~UavcanEscController()
Definition: esc.cpp:57
uint8_t _max_number_of_nonzero_outputs
Definition: esc.hpp:121
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uavcan::MethodBinder< UavcanEscController *, void(UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder
Definition: esc.hpp:101