PX4 Firmware
PX4 Autopilot Software http://px4.io
uavcanesc_main.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 PX4 Development Team. All rights reserved.
4  * Author: Pavel Kirienko <pavel.kirienko@gmail.com>
5  * David Sidrane <david_s5@nscdg.com>
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * 1. Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in
15  * the documentation and/or other materials provided with the
16  * distribution.
17  * 3. Neither the name PX4 nor the names of its contributors may be
18  * used to endorse or promote products derived from this software
19  * without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
28  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
29  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  ****************************************************************************/
35 
36 
37 #include <px4_platform_common/px4_config.h>
38 #include <px4_platform_common/log.h>
39 #include <px4_platform_common/tasks.h>
40 
41 #include <cstdlib>
42 #include <cstring>
43 #include <fcntl.h>
44 #include <systemlib/err.h>
45 #include <parameters/param.h>
46 #include <lib/mixer/MixerGroup.hpp>
47 #include <version/version.h>
48 #include <arch/board/board.h>
49 #include <arch/chip/chip.h>
50 
51 #include <drivers/drv_hrt.h>
52 #include <drivers/drv_pwm_output.h>
53 
54 #include "uavcanesc_main.hpp"
55 #include "led.hpp"
57 
58 #include "boot_app_shared.h"
59 
60 using namespace time_literals;
61 
62 /**
63  *
64  * Implements basic functionality of UAVCAN esc.
65  */
66 
67 /*
68  * This is the AppImageDescriptor used
69  * by the make_can_boot_descriptor.py tool to set
70  * the application image's descriptor so that the
71  * uavcan bootloader has the ability to validate the
72  * image crc, size etc of this application
73 */
74 
77  .image_crc = 0,
78  .image_size = 0,
79  .vcs_commit = 0,
80  .major_version = APP_VERSION_MAJOR,
81  .minor_version = APP_VERSION_MINOR,
82  .reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
83 };
84 
85 /*
86  * UavcanNode
87  */
89 
90 UavcanEsc::UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
91  CDev(UAVCAN_DEVICE_PATH),
92  active_bitrate(0),
93  _node(can_driver, system_clock),
94  _node_mutex(),
95  _fw_update_listner(_node),
96  _reset_timer(_node)
97 {
98  const int res = pthread_mutex_init(&_node_mutex, nullptr);
99 
100  if (res < 0) {
101  std::abort();
102  }
103 
104  px4_sem_init(&_sem, 0, 0);
105  /* semaphore use case is a signal */
106  px4_sem_setprotocol(&_sem, SEM_PRIO_NONE);
107 
108 }
109 
111 {
112  if (_task != -1) {
113  /* tell the task we want it to go away */
114  _task_should_exit = true;
115 
116  unsigned i = 10;
117 
118  do {
119  /* wait 5ms - it should wake every 10ms or so worst-case */
120  ::usleep(5000);
121 
122  /* if we have given up, kill it */
123  if (--i == 0) {
124  task_delete(_task);
125  break;
126  }
127 
128  } while (_task != -1);
129  }
130 
131  _instance = nullptr;
132  px4_sem_destroy(&_sem);
133 }
134 
135 int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate)
136 {
137 
138 
139  if (_instance != nullptr) {
140  PX4_WARN("Already started");
141  return -1;
142  }
143 
144  /*
145  * CAN driver init
146  */
147  static CanInitHelper can;
148  static bool can_initialized = false;
149 
150  if (!can_initialized) {
151  const int can_init_res = can.init(bitrate);
152 
153  if (can_init_res < 0) {
154  PX4_WARN("CAN driver init failed %i", can_init_res);
155  return can_init_res;
156  }
157 
158  can_initialized = true;
159  }
160 
161  /*
162  * Node init
163  */
165 
166  if (_instance == nullptr) {
167  PX4_WARN("Out of memory");
168  return -1;
169  }
170 
171 
172  const int node_init_res = _instance->init(node_id, can.driver.updateEvent());
173 
174  if (node_init_res < 0) {
175  delete _instance;
176  _instance = nullptr;
177  PX4_WARN("Node init failed %i", node_init_res);
178  return node_init_res;
179  }
180 
181 
182  /* Keep the bit rate for reboots on BenginFirmware updates */
183 
184  _instance->active_bitrate = bitrate;
185 
186  /*
187  * Start the task. Normally it should never exit.
188  */
189  static auto run_trampoline = [](int, char *[]) {return UavcanEsc::_instance->run();};
190  _instance->_task = px4_task_spawn_cmd("uavcanesc", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
191  static_cast<main_t>(run_trampoline), nullptr);
192 
193  if (_instance->_task < 0) {
194  PX4_WARN("start failed: %d", errno);
195  return -errno;
196  }
197 
198  return OK;
199 }
200 
202 {
203  /* software version */
204  uavcan::protocol::SoftwareVersion swver;
205 
206  // Extracting the first 8 hex digits of the git hash and converting them to int
207  char fw_git_short[9] = {};
208  std::memmove(fw_git_short, px4_firmware_version_string(), 8);
209  char *end = nullptr;
210  swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
211  swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
212  swver.major = AppDescriptor.major_version;
213  swver.minor = AppDescriptor.minor_version;
214  swver.image_crc = AppDescriptor.image_crc;
215 
216  PX4_WARN("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
217 
218  _node.setSoftwareVersion(swver);
219 
220  /* hardware version */
221  uavcan::protocol::HardwareVersion hwver;
222 
223  hwver.major = HW_VERSION_MAJOR;
224  hwver.minor = HW_VERSION_MINOR;
225 
226  mfguid_t mfgid = {};
227  board_get_mfguid(mfgid);
228  uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin());
229 
230  _node.setHardwareVersion(hwver);
231 }
232 
233 static void cb_reboot(const uavcan::TimerEvent &)
234 {
235  px4_systemreset(false);
236 
237 }
238 
239 void UavcanEsc::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanEsc::BeginFirmwareUpdate::Request>
240  &req,
241  uavcan::ServiceResponseDataStructure<UavcanEsc::BeginFirmwareUpdate::Response> &rsp)
242 {
243  static bool inprogress = false;
244 
245  rsp.error = rsp.ERROR_UNKNOWN;
246 
247  if (req.image_file_remote_path.path.size()) {
248  rsp.error = rsp.ERROR_IN_PROGRESS;
249 
250  if (!inprogress) {
251  inprogress = true;
253  shared.bus_speed = active_bitrate;
254  shared.node_id = _node.getNodeID().get();
256  rgb_led(255, 128, 0, 5);
257  _reset_timer.setCallback(cb_reboot);
258  _reset_timer.startOneShotWithDelay(uavcan::MonotonicDuration::fromMSec(1000));
259  rsp.error = rsp.ERROR_OK;
260  }
261  }
262 }
263 
264 int UavcanEsc::init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events)
265 {
266  int ret = -1;
267 
268  // Do regular cdev init
269  ret = CDev::init();
270 
271  if (ret != OK) {
272  return ret;
273  }
274 
275  _node.setName(HW_UAVCAN_NAME);
276 
277  _node.setNodeID(node_id);
278 
279  fill_node_info();
280 
281  const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this,
283 
284  if (srv_start_res < 0) {
285  return ret;
286  }
287 
288  bus_events.registerSignalCallback(UavcanEsc::busevent_signal_trampoline);
289 
290  return _node.start();
291 }
292 
293 
294 /*
295  * Restart handler
296  */
297 class RestartRequestHandler: public uavcan::IRestartRequestHandler
298 {
299  bool handleRestartRequest(uavcan::NodeID request_source) override
300  {
301  ::syslog(LOG_INFO, "UAVCAN: Restarting by request from %i\n", int(request_source.get()));
302  ::usleep(20 * 1000 * 1000);
303  px4_systemreset(false);
304  return true; // Will never be executed BTW
305  }
307 
309 {
310  const int spin_res = _node.spin(uavcan::MonotonicTime());
311 
312  if (spin_res < 0) {
313  PX4_WARN("node spin error %i", spin_res);
314  }
315 }
316 
317 static void signal_callback(void *arg)
318 {
319  /* Note: we are in IRQ context here */
320  px4_sem_t *sem = (px4_sem_t *)arg;
321  int semaphore_value;
322 
323  if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) {
324  return;
325  }
326 
327  px4_sem_post(sem);
328 }
329 
330 
331 void
333 {
334  if (_instance) {
336  }
337 }
338 
340 {
341 
342  get_node().setRestartRequestHandler(&restart_request_handler);
343 
344  while (init_indication_controller(get_node()) < 0) {
345  ::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n");
346  ::sleep(1);
347  }
348 
349  (void)pthread_mutex_lock(&_node_mutex);
350 
351  _node.setModeOperational();
352 
353  hrt_call timer_call{};
354  hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem);
355 
356  while (!_task_should_exit) {
357  // Mutex is unlocked while the thread is blocked on IO multiplexing
358  (void)pthread_mutex_unlock(&_node_mutex);
359 
360  while (px4_sem_wait(&_sem) != 0);
361 
362  (void)pthread_mutex_lock(&_node_mutex);
363 
364  node_spin_once(); // Non-blocking
365 
366  // Do Something
367 
368  }
369 
370  hrt_cancel(&timer_call);
371  teardown();
372  (void)pthread_mutex_unlock(&_node_mutex);
373 
374  exit(0);
375 }
376 
377 int
379 {
380  return 0;
381 }
382 
383 
384 
385 int
386 UavcanEsc::ioctl(file *filp, int cmd, unsigned long arg)
387 {
388  int ret = OK;
389 
390  lock();
391 
392  switch (cmd) {
393 
394 
395 
396  default:
397  ret = -ENOTTY;
398  break;
399  }
400 
401  unlock();
402 
403  if (ret == -ENOTTY) {
404  ret = CDev::ioctl(filp, cmd, arg);
405  }
406 
407  return ret;
408 }
409 
410 void
412 {
413  if (!_instance) {
414  PX4_WARN("not running, start first");
415  }
416 
417  (void)pthread_mutex_lock(&_node_mutex);
418 
419 
420  (void)pthread_mutex_unlock(&_node_mutex);
421 }
422 
423 /*
424  * App entry point
425  */
426 static void print_usage()
427 {
428  PX4_INFO("usage: \n"
429  "\tuavcanesc {start|status|stop}");
430 }
431 
432 extern "C" __EXPORT int uavcannode_start(int argc, char *argv[]);
433 
434 int uavcannode_start(int argc, char *argv[])
435 {
436  // CAN bitrate
437  int32_t bitrate = 0;
438  // Node ID
439  int32_t node_id = 0;
440 
441  // Did the bootloader auto baud and get a node ID Allocated
442 
444  int valid = bootloader_app_shared_read(&shared, BootLoader);
445 
446  if (valid == 0) {
447 
448  bitrate = shared.bus_speed;
449  node_id = shared.node_id;
450 
451  // Invalidate to prevent deja vu
452 
454 
455  } else {
456 
457  // Node ID
458  (void)param_get(param_find("ESC_NODE_ID"), &node_id);
459  (void)param_get(param_find("ESC_BITRATE"), &bitrate);
460  }
461 
462  if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
463  PX4_WARN("Invalid Node ID %i", node_id);
464  ::exit(1);
465  }
466 
467  // Start
468  PX4_WARN("Node ID %u, bitrate %u", node_id, bitrate);
469  int rv = UavcanEsc::start(node_id, bitrate);
470  ::sleep(1);
471  return rv;
472 }
473 
474 extern "C" __EXPORT int uavcanesc_main(int argc, char *argv[]);
475 int uavcanesc_main(int argc, char *argv[])
476 {
477  if (argc < 2) {
478  print_usage();
479  ::exit(1);
480  }
481 
482  if (!std::strcmp(argv[1], "start")) {
483 
484  if (UavcanEsc::instance()) {
485  errx(1, "already started");
486  }
487 
488  return uavcannode_start(argc, argv);
489  }
490 
491  /* commands below require the app to be started */
492  UavcanEsc *const inst = UavcanEsc::instance();
493 
494  if (!inst) {
495  PX4_ERR("application not running");
496  ::exit(1);
497 
498  }
499 
500  if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
501  inst->print_info();
502  ::exit(0);
503  }
504 
505  if (!std::strcmp(argv[1], "stop")) {
506  delete inst;
507  ::exit(0);
508  }
509 
510  print_usage();
511  ::exit(1);
512 }
uavcan::ServiceServer< BeginFirmwareUpdate, BeginFirmwareUpdateCallBack > _fw_update_listner
__EXPORT void bootloader_app_shared_invalidate(void)
static void cb_reboot(const uavcan::TimerEvent &)
#define UAVCAN_DEVICE_PATH
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock)
Helper class.
Definition: can.hpp:291
void lock()
Take the driver lock.
Definition: CDev.hpp:264
static SystemClock & instance()
Calls clock::init() as needed.
Definition: I2C.hpp:51
virtual int init()
Definition: CDev.cpp:119
void node_spin_once()
void print_info()
int init(uavcan::uint32_t bitrate)
This overload simply configures the provided bitrate.
Definition: can.hpp:310
static int start(uavcan::NodeID node_id, uint32_t bitrate)
__EXPORT int bootloader_app_shared_read(bootloader_app_shared_t *shared, eRole_t role)
High-resolution timer with callouts and timekeeping.
__EXPORT int uavcanesc_main(int argc, char *argv[])
Global flash based parameter store.
static void print_usage()
void init()
Activates/configures the hardware registers.
bool handleRestartRequest(uavcan::NodeID request_source) override
__EXPORT void bootloader_app_shared_write(bootloader_app_shared_t *shared, eRole_t role)
Node _node
library instance
static void busevent_signal_trampoline()
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
uavcan::MethodBinder< UavcanEsc *, void(UavcanEsc::*)(const uavcan::ReceivedDataStructure< UavcanEsc::BeginFirmwareUpdate::Request > &, uavcan::ServiceResponseDataStructure< UavcanEsc::BeginFirmwareUpdate::Response > &)> BeginFirmwareUpdateCallBack
__EXPORT int uavcannode_start(int argc, char *argv[])
__EXPORT void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
Call callout(arg) after delay, and then after every interval.
virtual int ioctl(file *filp, int cmd, unsigned long arg)
int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events)
#define APP_DESCRIPTOR_SIGNATURE
const char * px4_firmware_version_string(void)
Firmware version as human readable string (git tag)
Definition: version.c:338
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
Tools for system version detection.
uavcan::TimerEventForwarder< void(*)(const uavcan::TimerEvent &)> _reset_timer
static UavcanEsc * _instance
singleton pointer
int32_t active_bitrate
int init_indication_controller(uavcan::INode &node)
void cb_beginfirmware_update(const uavcan::ReceivedDataStructure< UavcanEsc::BeginFirmwareUpdate::Request > &req, uavcan::ServiceResponseDataStructure< UavcanEsc::BeginFirmwareUpdate::Response > &rsp)
static constexpr unsigned StackSize
struct @83::@85::@87 file
static UavcanEsc * instance()
virtual ~UavcanEsc()
pthread_mutex_t _node_mutex
#define boot_app_shared_section
#define errx(eval,...)
Definition: err.h:89
static void signal_callback(void *arg)
boot_app_shared_section app_descriptor_t AppDescriptor
Implements basic functionality of UAVCAN esc.
Callout record.
Definition: drv_hrt.h:72
int _task
handle to the OS task
bool _task_should_exit
flag to indicate to tear down the CAN driver
#define OK
Definition: uavcan_main.cpp:71
void rgb_led(int r, int g, int b, int freqs)
Definition: led.cpp:44
uint8_t signature[sizeof(uint64_t)]
BusEvent & updateEvent()
Definition: can.hpp:282
void fill_node_info()
void unlock()
Release the driver lock.
Definition: CDev.hpp:269
A UAVCAN node.
__EXPORT void hrt_cancel(struct hrt_call *entry)
Remove the entry from the callout list.
px4_sem_t _sem
semaphore for scheduling the task
RestartRequestHandler restart_request_handler
Node & get_node()