fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

emlid navio2: update RGB LED driver (move away from DriverFramework)

Browse Source 
- delete unused linux gpio wrapper
This commit is contained in:
Daniel Agar 2020-01-07 14:33:25 -05:00
parent ace1acca3c
commit f3cd5b19c8
12 changed files with 231 additions and 860 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
boards/emlid/navio2/navio_rgbled/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2016 PX4 Development Team. All rights reserved.
# Copyright (c) 2016-2020 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
@ -32,13 +32,12 @@
############################################################################
px4_add_module(
MODULE drivers__navio_rgbled
MODULE navio_rgbled
MAIN navio_rgbled
SRCS
navio_rgbled.cpp
NavioRGBLed.cpp
NavioRGBLed.hpp
DEPENDS
led
linux_gpio
px4_work_queue
)
#add_subdirectory(test)

174
boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp Normal file
View File

@ -0,0 +1,174 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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.
*
****************************************************************************/
#include "NavioRGBLed.hpp"
NavioRGBLed::NavioRGBLed() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
};
NavioRGBLed::~NavioRGBLed()
{
ScheduleClear();
_ledR.off();
_ledG.off();
_ledB.off();
}
int NavioRGBLed::init()
{
_ledR.off();
_ledG.off();
_ledB.off();
// kick off work queue
ScheduleNow();
return PX4_OK;
}
void NavioRGBLed::Run()
{
LedControlData led_control_data{};
if (_led_controller.update(led_control_data) == 1) {
switch (led_control_data.leds[0].color) {
case led_control_s::COLOR_RED:
_ledR.on();
_ledG.off();
_ledB.off();
break;
case led_control_s::COLOR_GREEN:
_ledR.off();
_ledG.on();
_ledB.off();
break;
case led_control_s::COLOR_BLUE:
_ledR.off();
_ledG.off();
_ledB.on();
break;
case led_control_s::COLOR_AMBER: // make it the same as yellow
case led_control_s::COLOR_YELLOW:
_ledR.on();
_ledG.on();
_ledB.off();
break;
case led_control_s::COLOR_PURPLE:
_ledR.on();
_ledG.off();
_ledB.on();
break;
case led_control_s::COLOR_CYAN:
_ledR.off();
_ledG.on();
_ledB.on();
break;
case led_control_s::COLOR_WHITE:
_ledR.on();
_ledG.on();
_ledB.on();
break;
default: // led_control_s::COLOR_OFF
_ledR.off();
_ledG.off();
_ledB.off();
break;
}
}
/* re-queue ourselves to run again later */
ScheduleDelayed(_led_controller.maximum_update_interval());
}
int NavioRGBLed::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int NavioRGBLed::task_spawn(int argc, char *argv[])
{
NavioRGBLed *instance = new NavioRGBLed();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init() == PX4_OK) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int NavioRGBLed::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Emlid Navio2 RGB LED driver.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("navio_rgbled", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int navio_rgbled_main(int argc, char *argv[])
{
return NavioRGBLed::main(argc, argv);
}

52
boards/emlid/navio2/navio_rgbled/navio_rgbled.h → boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2020 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
@ -30,28 +30,54 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/drivers/linux_gpio/linux_gpio.h>
#include <DevObj.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/led/led.h>
class RGBLED : public DriverFramework::DevObj
class NavioRGBLed : public ModuleBase<NavioRGBLed>, public px4::ScheduledWorkItem
{
public:
RGBLED(const char *name);
virtual ~RGBLED() = default;
NavioRGBLed();
~NavioRGBLed() override;
int start();
int stop();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
protected:
void _measure();
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
int init();
private:
void Run() override;
LedController _led_controller;
LinuxGPIO _gpioR;
LinuxGPIO _gpioG;
LinuxGPIO _gpioB;
class SysRGBLED
{
public:
explicit SysRGBLED(const char *path) : _fd(open(path, O_WRONLY)) {}
~SysRGBLED() { close(_fd); }
bool on() { return (write(_fd, "0", 1) > 0); }
bool off() { return (write(_fd, "1", 1) > 0); }
private:
int _fd{-1};
};
SysRGBLED _ledR{"/sys/class/leds/rgb_led0/brightness"};
SysRGBLED _ledG{"/sys/class/leds/rgb_led1/brightness"};
SysRGBLED _ledB{"/sys/class/leds/rgb_led2/brightness"};
};

278
boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp
View File

@ -1,278 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 - 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
* 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.
*
****************************************************************************/
#include <px4_platform_common/posix.h>
#include <drivers/drv_led.h>
#include <string.h>
#include "navio_rgbled.h"
#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
using namespace DriverFramework;
RGBLED::RGBLED(const char *name)
: DevObj(name,
RGBLED0_DEVICE_PATH,
RGBLED_BASE_DEVICE_PATH,
DeviceBusType_UNKNOWN,
0)
, _gpioR(4)
, _gpioG(27)
, _gpioB(6)
{
};
int RGBLED::start()
{
int res = DevObj::init();
if (res != 0) {
DF_LOG_ERR("could not init DevObj (%i)", res);
return res;
}
res = _gpioR.exportPin();
if (res != 0) {
PX4_ERR("red led: failed to export");
goto cleanup;
}
res = _gpioR.setDirection(LinuxGPIO::Direction::OUT);
if (res != 0) {
PX4_ERR("red led: failed to set direction");
goto cleanup;
}
res = _gpioG.exportPin();
if (res != 0) {
PX4_ERR("green led: failed to export");
goto cleanup;
}
res = _gpioG.setDirection(LinuxGPIO::Direction::OUT);
if (res != 0) {
PX4_ERR("green led: failed to set direction");
goto cleanup;
}
res = _gpioB.exportPin();
if (res != 0) {
PX4_ERR("blue led: failed to export");
goto cleanup;
}
res = _gpioB.setDirection(LinuxGPIO::Direction::OUT);
if (res != 0) {
PX4_ERR("blue led: failed to set direction");
goto cleanup;
}
// update at fixed interval
DevObj::setSampleInterval(_led_controller.maximum_update_interval());
res = DevObj::start();
if (res != 0) {
DF_LOG_ERR("could not start DevObj (%i)", res);
return res;
}
return res;
cleanup:
_gpioR.unexportPin();
_gpioG.unexportPin();
_gpioB.unexportPin();
return res;
}
int
RGBLED::stop()
{
_gpioR.unexportPin();
_gpioG.unexportPin();
_gpioB.unexportPin();
int res = DevObj::stop();
if (res < 0) {
DF_LOG_ERR("could not stop DevObj");
//this may not be an error for this device
return res;
}
return 0;
}
void
RGBLED::_measure()
{
LedControlData led_control_data;
if (_led_controller.update(led_control_data) == 1) {
switch (led_control_data.leds[0].color) {
case led_control_s::COLOR_RED:
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
case led_control_s::COLOR_GREEN:
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
case led_control_s::COLOR_BLUE:
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
case led_control_s::COLOR_AMBER: //make it the same as yellow
case led_control_s::COLOR_YELLOW:
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
case led_control_s::COLOR_PURPLE:
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
case led_control_s::COLOR_CYAN:
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
case led_control_s::COLOR_WHITE:
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
default: // led_control_s::COLOR_OFF
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
}
}
}
extern "C" { __EXPORT int navio_rgbled_main(int argc, char *argv[]); }
namespace navio_rgbled
{
int start();
int stop();
void usage();
RGBLED *g_dev = nullptr;
int start()
{
g_dev = new RGBLED("navio_rgbled");
if (g_dev == nullptr) {
PX4_ERR("failed instantiating RGBLED");
return -1;
}
return g_dev->start();
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("not running");
return -1;
}
g_dev->stop();
delete g_dev;
g_dev = nullptr;
return 0;
}
void usage()
{
PX4_INFO("Usage: navio_rgbled 'start', 'stop'");
}
} //namespace navio_rgbled
int navio_rgbled_main(int argc, char *argv[])
{
int ret = 0;
int myoptind = 1;
if (argc <= 1) {
navio_rgbled::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = navio_rgbled::start();
}
else if (!strcmp(verb, "stop")) {
ret = navio_rgbled::stop();
}
else {
navio_rgbled::usage();
return 1;
}
return ret;
}

44
boards/emlid/navio2/navio_rgbled/test/CMakeLists.txt
View File

@ -1,44 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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.
#
############################################################################
include_directories(..)
add_executable(navio_rgbled_test
main.cpp
test.cpp
)
target_link_libraries(navio_rgbled_test
drivers__navio_rgbled
drivers__navio_gpio
df_driver_framework
)

39
boards/emlid/navio2/navio_rgbled/test/main.cpp
View File

@ -1,39 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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.
*
****************************************************************************/
extern int do_test();
int main()
{
return do_test();
}

113
boards/emlid/navio2/navio_rgbled/test/test.cpp
View File

@ -1,113 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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.
*
****************************************************************************/
#include <px4_platform_common/defines.h>
#include <stdio.h>
#include <unistd.h>
#include <DevMgr.hpp>
#include <drivers/drv_rgbled.h>
#include "navio_rgbled.h"
using namespace DriverFramework;
int do_test();
int do_test()
{
DevHandle h;
RGBLED *g_dev = nullptr;
if (Framework::initialize() < 0) {
printf("Framework init failed\n");
return -1;
}
g_dev = new RGBLED("navio_rgbled test");
g_dev->start();
DevMgr::getHandle(RGBLED0_DEVICE_PATH, h);
if (!h.isValid()) {
printf("No RGB LED at " RGBLED0_DEVICE_PATH);
return -1;
}
printf("off\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_OFF);
sleep(2);
printf("red\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_RED);
sleep(2);
printf("yellow\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_YELLOW);
sleep(2);
printf("purple\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_PURPLE);
sleep(2);
printf("green\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_GREEN);
sleep(2);
printf("blue\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_BLUE);
sleep(2);
printf("blue blink slow\n");
h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_SLOW);
sleep(10);
printf("green blink normal\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_GREEN);
h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL);
sleep(10);
printf("red blink fast\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_RED);
h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_FAST);
sleep(10);
printf("blue breathe (bogus)\n");
h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_BLUE);
h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BREATHE);
sleep(10);
return 0;
}

1
src/lib/drivers/CMakeLists.txt
View File

@ -37,7 +37,6 @@ add_subdirectory(barometer)
add_subdirectory(device)
add_subdirectory(gyroscope)
add_subdirectory(led)
add_subdirectory(linux_gpio)
add_subdirectory(magnetometer)
add_subdirectory(rangefinder)
add_subdirectory(smbus)

38
src/lib/drivers/linux_gpio/CMakeLists.txt
View File

@ -1,38 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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.
#
############################################################################
px4_add_library(linux_gpio
linux_gpio.cpp
)
#add_subdirectory(test)

241
src/lib/drivers/linux_gpio/linux_gpio.cpp
View File

@ -1,241 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 - 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
* 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.
*
****************************************************************************/
#include "linux_gpio.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <px4_platform_common/posix.h>
#define PIN_INDEX_BUFFER_MAX (16)
#define PIN_DIRECTION_BUFFER_MAX (30 + PIN_INDEX_BUFFER_MAX)
#define PIN_VALUE_BUFFER_MAX (26 + PIN_INDEX_BUFFER_MAX)
LinuxGPIO::LinuxGPIO(unsigned int pin)
: _pin(pin)
, _fd(-1)
{
}
LinuxGPIO::~LinuxGPIO()
{
if (_fd != -1) {
close(_fd);
}
}
int LinuxGPIO::exportPin()
{
int ret;
char pinIndex[PIN_INDEX_BUFFER_MAX];
char valuePath[PIN_VALUE_BUFFER_MAX];
int fd = -1;
int bytes_to_write;
struct stat statbuf;
/* If GPIO was already opened, close it */
if (_fd != -1) {
close(_fd);
_fd = -1;
}
/* Check if GPIO is already exported */
snprintf(valuePath, PIN_VALUE_BUFFER_MAX, "/sys/class/gpio/gpio%d/value", _pin);
ret = stat(valuePath, &statbuf);
if (ret == -1) {
/* GPIO is not exported */
fd = open("/sys/class/gpio/export", O_WRONLY);
if (fd == -1) {
int err = errno;
PX4_ERR("export %u: open: %s (%d)", _pin, strerror(err), err);
return -1;
}
bytes_to_write = snprintf(pinIndex, PIN_INDEX_BUFFER_MAX, "%u", _pin);
ret = write(fd, pinIndex, bytes_to_write);
if (ret == -1) {
int err = errno;
PX4_ERR("export %u: write: %s (%d)", _pin, strerror(err), err);
goto cleanup;
} else if (ret != bytes_to_write) {
PX4_ERR("export %u: write incomplete %d != %d", _pin, ret, bytes_to_write);
goto cleanup;
}
}
_fd = open(valuePath, O_RDWR);
if (_fd == -1) {
int err = errno;
ret = -1;
PX4_ERR("export %u: open: %s (%d)", _pin, strerror(err), err);
goto cleanup;
}
ret = 0;
cleanup:
if (fd != -1) {
close(fd);
}
return ret;
}
int LinuxGPIO::unexportPin()
{
char pinIndex[PIN_INDEX_BUFFER_MAX];
int fd;
int ret;
int bytes_to_write;
if (_fd != -1) {
close(_fd);
_fd = -1;
}
fd = open("/sys/class/gpio/unexport", O_WRONLY);
if (fd == -1) {
int err = errno;
PX4_ERR("unexport %u: open: %s (%d)", _pin, strerror(err), err);
return -1;
}
bytes_to_write = snprintf(pinIndex, PIN_INDEX_BUFFER_MAX, "%u", _pin);
ret = write(fd, pinIndex, bytes_to_write);
if (ret == -1) {
int err = errno;
PX4_ERR("unexport %u: write: %s (%d)", _pin, strerror(err), err);
goto cleanup;
} else if (ret != bytes_to_write) {
PX4_ERR("unexport %u: write incomplete %d != %d", _pin, ret, bytes_to_write);
goto cleanup;
}
ret = 0;
cleanup:
close(fd);
return ret;
}
int LinuxGPIO::setDirection(LinuxGPIO::Direction dir)
{
char path[PIN_DIRECTION_BUFFER_MAX];
int fd;
int ret;
snprintf(path, PIN_DIRECTION_BUFFER_MAX, "/sys/class/gpio/gpio%d/direction", _pin);
fd = open(path, O_WRONLY);
if (fd == -1) {
int err = errno;
PX4_ERR("dir %u: open: %s (%d)", _pin, strerror(err), err);
return -1;
}
if (dir == Direction::IN) {
ret = write(fd, "in", 2);
} else {
ret = write(fd, "out", 3);
}
if (ret == -1) {
int err = errno;
PX4_ERR("dir %u: write: %s (%d)", _pin, strerror(err), err);
goto cleanup;
}
ret = 0;
cleanup:
close(fd);
return ret;
}
int LinuxGPIO::readValue()
{
char buf[2];
int ret;
ret = ::read(_fd, buf, sizeof(buf));
if (ret == -1) {
int err = errno;
PX4_ERR("readValue %u: read: %s (%d)", _pin, strerror(err), err);
return ret;
}
ret = strtol(buf, nullptr, 10);
return ret;
}
int LinuxGPIO::writeValue(LinuxGPIO::Value value)
{
char buf[2];
int ret;
if (value != Value::LOW && value != Value::HIGH) {
return -EINVAL;
}
int buflen = snprintf(buf, sizeof(buf), "%u", (unsigned int)value);
ret = ::write(_fd, buf, buflen);
if (ret == -1) {
int err = errno;
PX4_ERR("writeValue %u: write: %s (%d)", _pin, strerror(err), err);
return ret;
}
return 0;
}

71
src/lib/drivers/linux_gpio/linux_gpio.h
View File

@ -1,71 +0,0 @@
/****************************************************************************
*
* 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
* 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.
*
****************************************************************************/
/**
* @file linux_gpio.h
*
* Linux sysfs GPIO Interface
*
* This interface allows manipulation of sysfs GPIOs on Linux
*
* @author Nicolae Rosia <nicolae.rosia@gmail.com>
*/
#pragma once
class LinuxGPIO
{
public:
LinuxGPIO(unsigned int pin);
~LinuxGPIO();
enum class Direction {
IN = 0,
OUT = 1,
};
enum class Value {
LOW = 0,
HIGH = 1,
};
int exportPin();
int unexportPin();
int setDirection(LinuxGPIO::Direction dir);
int readValue();
int writeValue(LinuxGPIO::Value value);
private:
int _pin;
int _fd;
};

29
src/modules/commander/commander_helper.cpp
View File

@ -63,9 +63,6 @@
#include <drivers/drv_tone_alarm.h>
#include "commander_helper.h"
#include "DevMgr.hpp"
using namespace DriverFramework;
#define VEHICLE_TYPE_FIXED_WING 1
#define VEHICLE_TYPE_QUADROTOR 2
@ -131,8 +128,8 @@ static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for rep
static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end
static unsigned int tune_durations[TONE_NUMBER_OF_TUNES] {};
static DevHandle h_leds;
static DevHandle h_buzzer;
static int fd_leds{-1};
static led_control_s led_control {};
static orb_advert_t led_control_pub = nullptr;
static tune_control_s tune_control {};
@ -296,25 +293,25 @@ int led_init()
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
/* first open normal LEDs */
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR);
if (!h_leds.isValid()) {
PX4_WARN("LED: getHandle fail\n");
if (fd_leds < 0) {
PX4_ERR("LED: open %s failed", LED0_DEVICE_PATH);
return PX4_ERROR;
}
/* the green LED is only available on FMUv5 */
(void)h_leds.ioctl(LED_ON, LED_GREEN);
px4_ioctl(fd_leds, LED_ON, LED_GREEN);
/* the blue LED is only available on AeroCore but not FMUv2 */
(void)h_leds.ioctl(LED_ON, LED_BLUE);
px4_ioctl(fd_leds, LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (h_leds.ioctl(LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail\n");
if (px4_ioctl(fd_leds, LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail");
return PX4_ERROR;
}
@ -327,22 +324,22 @@ int led_init()
void led_deinit()
{
orb_unadvertise(led_control_pub);
DevMgr::releaseHandle(h_leds);
px4_close(fd_leds);
}
int led_toggle(int led)
{
return h_leds.ioctl(LED_TOGGLE, led);
return px4_ioctl(fd_leds, LED_TOGGLE, led);
}
int led_on(int led)
{
return h_leds.ioctl(LED_ON, led);
return px4_ioctl(fd_leds, LED_ON, led);
}
int led_off(int led)
{
return h_leds.ioctl(LED_OFF, led);
return px4_ioctl(fd_leds, LED_OFF, led);
}
void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)