diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.cpp b/src/drivers/rpi_rc_in/rpi_rc_in.cpp index d9f5581d7d..bdd7dee6e8 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.cpp +++ b/src/drivers/rpi_rc_in/rpi_rc_in.cpp @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2015 Mark Charl. All rights reserved. - * Copyright (C) 2017 Fan.zhang. All rights reserved. 421395590@qq.com - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2017 Fan.zhang. All rights reserved. 421395590@qq.com + * Copyright (c) 2017 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 @@ -35,20 +34,30 @@ #include "rpi_rc_in.h" using namespace rpi_rc_in; -//---------------------------------------------------------------------------------------------------------// + +RcInput::~RcInput() +{ + if (_mem) { + shmdt(_mem); + _mem = nullptr; + } + + work_cancel(HPWORK, &_work); + _is_running = false; +} + int RcInput::rpi_rc_init() { int i; //--------------初始化共享内存映射----------------------------// - if ((this->shmid = shmget(this->key, sizeof(int) * this->_channels, 0666)) - < 0) { - PX4_WARN("无法访问共享内存。Faild to access shared memory"); + if ((_shmid = shmget(_key, sizeof(int) * _channels, 0666)) < 0) { + PX4_WARN("Faild to access shared memory"); return -1; } - if ((this->mem = (int *) shmat(this->shmid, NULL, 0)) == (void *) - 1) { - PX4_WARN("无法映射共享内存。Faild to mapping shared memory"); + if ((_mem = (int *) shmat(_shmid, NULL, 0)) == (void *) - 1) { + PX4_WARN("Faild to map shared memory"); return -1; } @@ -57,16 +66,8 @@ int RcInput::rpi_rc_init() _data.values[i] = UINT16_MAX; } - _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); - - if (_rcinput_pub == nullptr) { - PX4_WARN("error: advertise failed"); - return -1; - } - return 0; } -//---------------------------------------------------------------------------------------------------------// int RcInput::start() { int result = 0; @@ -78,38 +79,37 @@ int RcInput::start() return -1; } - _isRunning = true; - result = work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, - this, 0); + _is_running = true; + result = work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, this, 0); if (result == -1) { - _isRunning = false; + _is_running = false; } return result; } -//---------------------------------------------------------------------------------------------------------// + void RcInput::stop() { - _shouldExit = true; + _should_exit = true; } -//---------------------------------------------------------------------------------------------------------// + void RcInput::cycle_trampoline(void *arg) { RcInput *dev = reinterpret_cast(arg); dev->_cycle(); } -//---------------------------------------------------------------------------------------------------------// + void RcInput::_cycle() { _measure(); - if (!_shouldExit) { + if (!_should_exit) { work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, this, USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); } } -//---------------------------------------------------------------------------------------------------------// + void RcInput::_measure(void) { uint64_t ts; @@ -118,7 +118,8 @@ void RcInput::_measure(void) int i = 0; for (i = 0; i < _channels; ++i) { - _data.values[i] = (*(this->mem + i) <= 0) ? UINT16_MAX : *(this->mem + i); + int value = _mem[i]; // access the shared memory (with a single read) + _data.values[i] = (value <= 0) ? UINT16_MAX : value; } ts = hrt_absolute_time(); @@ -141,10 +142,6 @@ void RcInput::_measure(void) orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); } } -//---------------------------------------------------------------------------------------------------------// -/** - * Print the correct usage. - */ static void rpi_rc_in::usage(const char *reason) { @@ -152,9 +149,9 @@ static void rpi_rc_in::usage(const char *reason) PX4_ERR("%s", reason); } - PX4_INFO("用法: rpi_rc_in {start|stop|status}"); + PX4_INFO("rpi_rc_in {start|stop|status}"); } -//---------------------------------------------------------------------------------------------------------// + int rpi_rc_in_main(int argc, char **argv) { if (argc < 2) { @@ -164,8 +161,8 @@ int rpi_rc_in_main(int argc, char **argv) if (!strcmp(argv[1], "start")) { - if (rc_input != nullptr && rc_input->isRunning()) { - PX4_WARN("运行中。running"); + if (rc_input != nullptr && rc_input->is_running()) { + PX4_INFO("already running"); /* this is not an error */ return 0; } @@ -174,14 +171,14 @@ int rpi_rc_in_main(int argc, char **argv) // Check if alloc worked. if (nullptr == rc_input) { - PX4_ERR("遥控输入模块初始化错误。Rc input moduel initialization faild"); + PX4_ERR("Rc input module initialization faild"); return -1; } int ret = rc_input->start(); if (ret != 0) { - PX4_ERR("遥控输入模块未能启动。 Rc input module failure"); + PX4_ERR("Rc input module failure"); } return 0; @@ -189,8 +186,8 @@ int rpi_rc_in_main(int argc, char **argv) if (!strcmp(argv[1], "stop")) { - if (rc_input == nullptr || !rc_input->isRunning()) { - PX4_WARN("模块未运行。 Not runing"); + if (rc_input == nullptr || !rc_input->is_running()) { + PX4_WARN("Not running"); /* this is not an error */ return 0; } @@ -204,7 +201,7 @@ int rpi_rc_in_main(int argc, char **argv) /* wait up to 3s */ usleep(100000); - } while (rc_input->isRunning() && ++i < 30); + } while (rc_input->is_running() && ++i < 30); delete rc_input; rc_input = nullptr; @@ -213,18 +210,18 @@ int rpi_rc_in_main(int argc, char **argv) } if (!strcmp(argv[1], "status")) { - if (rc_input != nullptr && rc_input->isRunning()) { - PX4_INFO("运行中。 running"); + if (rc_input != nullptr && rc_input->is_running()) { + PX4_INFO("running"); } else { - PX4_INFO("未运行。 Not runing\n"); + PX4_INFO("Not running"); } return 0; } - usage("不知道你要做什么。 rpi_rc_in start|stop|status"); + usage("rpi_rc_in start|stop|status"); return 1; } -//---------------------------------------------------------------------------------------------------------// + diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.h b/src/drivers/rpi_rc_in/rpi_rc_in.h index 1835a144c4..def9f74a47 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.h +++ b/src/drivers/rpi_rc_in/rpi_rc_in.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2015 Mark Charl. All rights reserved. - * Copyright (C) 2017 Fan.zhang. All rights reserved. 421395590@qq.com - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2017 Fan.zhang. All rights reserved. 421395590@qq.com + * Copyright (c) 2017 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 @@ -33,6 +32,13 @@ * ****************************************************************************/ +#pragma once + +/*! @file rpi_rc_in.h + * Raspberry Pi driver to publish RC input from shared memory. + * It requires the ppmdecode program (https://github.com/crossa/raspberry-pi-ppm-rc-in) + */ + #include #include #include @@ -49,55 +55,48 @@ #include #include + #define RCINPUT_MEASURE_INTERVAL_US 20000 + namespace rpi_rc_in { class RcInput { public: - int *mem; - key_t key; - int shmid; - RcInput() : - mem(nullptr), key(4096), shmid(0), _shouldExit(false), _isRunning(false), _work { }, _rcinput_pub( - nullptr), _channels(8),//D8R-II plus - _data { } - { - //memset(_ch_fd, 0, sizeof(_ch_fd)); - } - ~RcInput() - { - this->mem = nullptr; - work_cancel(HPWORK, &_work); - _isRunning = false; - } + RcInput() = default; - /* @return 0 on success, -errno on failure */ + ~RcInput(); + + /** @return 0 on success, -errno on failure */ int start(); - /* @return 0 on success, -errno on failure */ + /** @return 0 on success, -errno on failure */ void stop(); - /* Trampoline for the work queue. */ + /** Trampoline for the work queue. */ static void cycle_trampoline(void *arg); - bool isRunning() + bool is_running() { - return _isRunning; + return _is_running; } private: void _cycle(); void _measure(); - bool _shouldExit; bool _isRunning; - struct work_s _work; - orb_advert_t _rcinput_pub; - int _channels; - //int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS]; - struct input_rc_s _data; - int rpi_rc_init(); + + bool _should_exit = false; + bool _is_running = false; + struct work_s _work = {}; + orb_advert_t _rcinput_pub = nullptr; + int _channels = 8; //D8R-II plus + struct input_rc_s _data = {}; + + int *_mem = nullptr; + key_t _key = 4096; ///< shared memory key (matches the ppmdecode program's key) + int _shmid = 0; }; static void usage(const char *reason);