rpi_rc_in: code cleanup

2026-06-01 02:55:07 +08:00 · 2017-05-08 11:32:58 +02:00
parent 785b997a2e
commit d898b555d3
2 changed files with 73 additions and 77 deletions
@@ -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<RcInput *>(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;

 }
-//---------------------------------------------------------------------------------------------------------//
+
@@ -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 <sys/types.h>
 #include <sys/stat.h>
 #include <sys/ipc.h>
@@ -49,55 +55,48 @@

 #include <uORB/uORB.h>
 #include <uORB/topics/input_rc.h>
+
 #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);