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

replace navio_gpio with linux_gpio

Browse Source 
Signed-off-by: Nicolae Rosia <nicolae.rosia@gmail.com>
This commit is contained in:
Nicolae Rosia 2017-05-14 17:49:03 +03:00 committed by Lorenz Meier
parent 3d77102653
commit 4af7036a8a
12 changed files with 383 additions and 591 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cmake/configs/posix_rpi_common.cmake
View File

@ -75,7 +75,7 @@ set(config_module_list
drivers/navio_adc
drivers/navio_sysfs_rc_in
drivers/navio_sysfs_pwm_out
drivers/navio_gpio
drivers/linux_gpio
drivers/navio_rgbled
drivers/pwm_out_sim

1
cmake/configs/posix_sitl_default.cmake
View File

@ -9,6 +9,7 @@ set(config_module_list
drivers/gps
drivers/pwm_out_sim
drivers/vmount
drivers/linux_gpio
modules/sensors
platforms/posix/drivers/accelsim
platforms/posix/drivers/adcsim

5
src/drivers/navio_gpio/CMakeLists.txt → src/drivers/linux_gpio/CMakeLists.txt
View File

@ -31,12 +31,11 @@
#
############################################################################
px4_add_module(
MODULE drivers__navio_gpio
MAIN navio_gpio
MODULE drivers__linux_gpio
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
navio_gpio.cpp
linux_gpio.cpp
DEPENDS
platforms__common
)

253
src/drivers/linux_gpio/linux_gpio.cpp Normal file
View File

@ -0,0 +1,253 @@
/****************************************************************************
*
* 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 <drivers/linux_gpio/linux_gpio.h>
#include <px4_posix.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.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)
{
}
int LinuxGPIO::exportPin()
{
return LinuxGPIO::_exportPin(_pin);
}
int LinuxGPIO::unexportPin()
{
return LinuxGPIO::_unexportPin(_pin);
}
int LinuxGPIO::setDirection(LinuxGPIO::Direction dir)
{
return LinuxGPIO::_setDirection(_pin, (int)dir);
}
int LinuxGPIO::readValue()
{
return LinuxGPIO::_readValue(_pin);
}
int LinuxGPIO::writeValue(LinuxGPIO::Value value)
{
return LinuxGPIO::_writeValue(_pin, (unsigned int)value);
}
int LinuxGPIO::_exportPin(unsigned int pin)
{
char pinIndex[PIN_INDEX_BUFFER_MAX];
int fd;
int ret;
int bytes_to_write;
fd = open("/sys/class/gpio/export", O_WRONLY);
if (fd == -1) {
int err = errno;
PX4_ERR("export failed: open: %s (%d)", 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 failed: write: %s (%d)", strerror(err), err);
goto cleanup;
} else if (ret != bytes_to_write) {
PX4_ERR("failed to write: incomplete %d != %d", ret, bytes_to_write);
goto cleanup;
}
ret = 0;
cleanup:
close(fd);
return ret;
}
int LinuxGPIO::_unexportPin(unsigned int pin)
{
char pinIndex[PIN_INDEX_BUFFER_MAX];
int fd;
int ret;
int bytes_to_write;
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(unsigned int pin, int 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 == 0) {
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(int pin)
{
char path[PIN_VALUE_BUFFER_MAX];
char buf[2];
int fd;
int ret;
snprintf(path, PIN_VALUE_BUFFER_MAX, "/sys/class/gpio/gpio%d/value", pin);
fd = open(path, O_RDONLY);
if (fd == -1) {
int err = errno;
PX4_ERR("read %u: open: %s (%d)", pin, strerror(err), err);
return -1;
}
ret = read(fd, buf, sizeof(buf));
if (ret == -1) {
int err = errno;
PX4_ERR("read %u: write: %s (%d)", pin, strerror(err), err);
goto cleanup;
}
ret = 0;
ret = strtol(buf, NULL, 10);
cleanup:
close(fd);
return ret;
}
int LinuxGPIO::_writeValue(int pin, unsigned int value)
{
char path[PIN_VALUE_BUFFER_MAX];
char buf[2];
int fd;
int ret;
if (value != (unsigned int)Value::LOW && value != (unsigned int)Value::HIGH) {
return -EINVAL;
}
snprintf(path, PIN_VALUE_BUFFER_MAX, "/sys/class/gpio/gpio%d/value", pin);
fd = open(path, O_WRONLY);
if (fd == -1) {
int err = errno;
PX4_ERR("set %u: open: %s (%d)", pin, strerror(err), err);
return -1;
}
int buflen = snprintf(buf, sizeof(buf), "%u", value);
ret = write(fd, buf, buflen);
if (ret == -1) {
int err = errno;
PX4_ERR("set %u: write: %s (%d)", pin, strerror(err), err);
goto cleanup;
}
ret = 0;
cleanup:
close(fd);
return ret;
}

46
src/drivers/navio_gpio/test/main.cpp → src/drivers/linux_gpio/linux_gpio.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* 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
@ -31,9 +31,45 @@
*
****************************************************************************/
extern int do_test();
/**
* @file linux_gpio.h
*
* Linux sysfs GPIO Interface
*
* This interface allows manipulation of sysfs GPIOs on Linux
*
* @author Nicolae Rosia <nicolae.rosia@gmail.com>
*/
int main()
class LinuxGPIO
{
return do_test();
}
public:
LinuxGPIO(unsigned int pin);
~LinuxGPIO() = default;
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:
static int _exportPin(unsigned int pin);
static int _unexportPin(unsigned int pin);
static int _setDirection(unsigned int pin, int dir);
static int _readValue(int pin);
static int _writeValue(int pin, unsigned int value);
int _pin;
};

246
src/drivers/navio_gpio/navio_gpio.cpp
View File

@ -1,246 +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_posix.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>
#include "navio_gpio.h"
namespace navio_gpio
{
extern "C" __EXPORT int navio_gpio_main(int argc, char *argv[]);
int Gpio::start(void)
{
int mem_fd;
if ((mem_fd = open("/dev/gpiomem", O_RDWR | O_SYNC)) < 0) {
PX4_WARN("failed to open gpiomem");
return -1;
}
if ((_gpio_map = mmap(NULL,
GPIO_BLOCK_SIZE,
PROT_READ | PROT_WRITE,
MAP_SHARED,
mem_fd,
GPIO_PHYS_ADDR)) == MAP_FAILED) {
PX4_WARN("failed to mmap GPIO region");
close(mem_fd);
return -1;
}
close(mem_fd);
return 0;
}
int Gpio::stop(void)
{
if (munmap(_gpio_map, GPIO_BLOCK_SIZE) < 0) {
return -1;
}
return 0;
}
void Gpio::atomic_modify(uintptr_t addr,
unsigned int shift,
unsigned int mask,
unsigned int value)
{
uint32_t tmp;
m_lock.lock();
tmp = *(volatile uint32_t *)addr;
tmp = (tmp & ~(mask << shift)) | (value << shift);
*(volatile uint32_t *)addr = tmp;
m_lock.unlock();
}
int Gpio::configgpio(uint32_t pinset)
{
unsigned int pin;
unsigned int cnf;
uintptr_t addr;
unsigned int shift;
pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT;
cnf = (pinset & GPIO_CNF_MASK) >> GPIO_CNF_SHIFT;
addr = (uintptr_t)_gpio_map + GPIO_GPFSEL0_OFFSET + pin / 10;
shift = (pin % 10) * 3;
atomic_modify(addr, shift, GPIO_CNF_MASK >> GPIO_CNF_SHIFT, cnf);
return 0;
}
int Gpio::unconfiggpio(uint32_t pinset)
{
return Gpio::configgpio(pinset);
}
bool Gpio::gpioread(uint32_t pinset)
{
unsigned int pin;
uintptr_t addr;
unsigned int shift;
pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT;
addr = (uintptr_t)_gpio_map + GPIO_GPLEV0_OFFSET + pin / 32;
shift = pin % 32;
return (*(volatile uint32_t *)addr >> shift) & 0x1u;
}
void Gpio::gpiowrite(uint32_t pinset, bool value)
{
unsigned int pin;
uintptr_t addr;
unsigned int shift;
pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT;
if (value == 0) {
addr = (uintptr_t)_gpio_map + GPIO_GPCLR0_OFFSET + pin / 32;
} else {
addr = (uintptr_t)_gpio_map + GPIO_GPSET0_OFFSET + pin / 32;
}
shift = pin % 32;
*(volatile uint32_t *)addr = 0x1u << shift;
}
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason) {
PX4_ERR("%s", reason);
}
PX4_INFO("usage: navio_gpio {start|stop|status}");
}
static Gpio *gpio = nullptr;
int navio_gpio_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
return 1;
}
if (!strcmp(argv[1], "start")) {
PX4_WARN("gpio should only be started directly for debugging");
if (gpio != nullptr && gpio->isMapped()) {
PX4_WARN("already mapped");
return 0;
}
gpio = new Gpio();
if (gpio == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
int ret = gpio->start();
if (ret != 0) {
PX4_ERR("start failed");
return 1;
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (gpio == nullptr) {
PX4_WARN("gpio not started from navio_gpio");
return 0;
}
if (!gpio->isMapped()) {
PX4_WARN("not mapped");
return 1;
}
gpio->stop();
delete gpio;
gpio = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (gpio == nullptr) {
PX4_WARN("gpio not started from navio_gpio");
return 0;
}
if (gpio->isMapped()) {
PX4_INFO("mapped");
} else {
PX4_INFO("not mapped\n");
}
return 0;
}
usage("unrecognized command");
return 1;
}
} // navio_gpio

149
src/drivers/navio_gpio/navio_gpio.h
View File

@ -1,149 +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 "SyncObj.hpp"
#include <stdint.h>
#define GPIO_BLOCK_SIZE (4096)
#define GPIO_PHYS_ADDR (0x3F200000)
#define GPIO_GPFSEL0_OFFSET (0x0)
#define GPIO_GPLEV0_OFFSET (0x34)
#define GPIO_GPSET0_OFFSET (0x1C)
#define GPIO_GPCLR0_OFFSET (0x28)
#define GPIO_CNF_SHIFT 6
#define GPIO_CNF_MASK (3 << GPIO_CNF_SHIFT)
#define GPIO_CNF_INPUT (0 << GPIO_CNF_SHIFT)
#define GPIO_CNF_OUTPUT (1 << GPIO_CNF_SHIFT)
#define GPIO_CNF_AF0 (4 << GPIO_CNF_SHIFT)
#define GPIO_CNF_AF1 (5 << GPIO_CNF_SHIFT)
#define GPIO_CNF_AF2 (6 << GPIO_CNF_SHIFT)
#define GPIO_CNF_AF3 (7 << GPIO_CNF_SHIFT)
#define GPIO_CNF_AF4 (3 << GPIO_CNF_SHIFT)
#define GPIO_CNF_AF5 (2 << GPIO_CNF_SHIFT)
#define GPIO_PIN_SHIFT 0
#define GPIO_PIN_MASK (63 << GPIO_PIN_SHIFT)
#define GPIO_PIN0 (0 << GPIO_PIN_SHIFT)
#define GPIO_PIN1 (1 << GPIO_PIN_SHIFT)
#define GPIO_PIN2 (2 << GPIO_PIN_SHIFT)
#define GPIO_PIN3 (3 << GPIO_PIN_SHIFT)
#define GPIO_PIN4 (4 << GPIO_PIN_SHIFT)
#define GPIO_PIN5 (5 << GPIO_PIN_SHIFT)
#define GPIO_PIN6 (6 << GPIO_PIN_SHIFT)
#define GPIO_PIN7 (7 << GPIO_PIN_SHIFT)
#define GPIO_PIN8 (8 << GPIO_PIN_SHIFT)
#define GPIO_PIN9 (9 << GPIO_PIN_SHIFT)
#define GPIO_PIN10 (10 << GPIO_PIN_SHIFT)
#define GPIO_PIN11 (11 << GPIO_PIN_SHIFT)
#define GPIO_PIN12 (12 << GPIO_PIN_SHIFT)
#define GPIO_PIN13 (13 << GPIO_PIN_SHIFT)
#define GPIO_PIN14 (14 << GPIO_PIN_SHIFT)
#define GPIO_PIN15 (15 << GPIO_PIN_SHIFT)
#define GPIO_PIN16 (16 << GPIO_PIN_SHIFT)
#define GPIO_PIN17 (17 << GPIO_PIN_SHIFT)
#define GPIO_PIN18 (18 << GPIO_PIN_SHIFT)
#define GPIO_PIN19 (19 << GPIO_PIN_SHIFT)
#define GPIO_PIN20 (20 << GPIO_PIN_SHIFT)
#define GPIO_PIN21 (21 << GPIO_PIN_SHIFT)
#define GPIO_PIN22 (22 << GPIO_PIN_SHIFT)
#define GPIO_PIN23 (23 << GPIO_PIN_SHIFT)
#define GPIO_PIN24 (24 << GPIO_PIN_SHIFT)
#define GPIO_PIN25 (25 << GPIO_PIN_SHIFT)
#define GPIO_PIN26 (26 << GPIO_PIN_SHIFT)
#define GPIO_PIN27 (27 << GPIO_PIN_SHIFT)
#define GPIO_PIN28 (28 << GPIO_PIN_SHIFT)
#define GPIO_PIN29 (29 << GPIO_PIN_SHIFT)
#define GPIO_PIN30 (30 << GPIO_PIN_SHIFT)
#define GPIO_PIN31 (31 << GPIO_PIN_SHIFT)
#define GPIO_PIN32 (32 << GPIO_PIN_SHIFT)
#define GPIO_PIN33 (33 << GPIO_PIN_SHIFT)
#define GPIO_PIN34 (34 << GPIO_PIN_SHIFT)
#define GPIO_PIN35 (35 << GPIO_PIN_SHIFT)
#define GPIO_PIN36 (36 << GPIO_PIN_SHIFT)
#define GPIO_PIN37 (37 << GPIO_PIN_SHIFT)
#define GPIO_PIN38 (38 << GPIO_PIN_SHIFT)
#define GPIO_PIN39 (39 << GPIO_PIN_SHIFT)
#define GPIO_PIN40 (40 << GPIO_PIN_SHIFT)
#define GPIO_PIN41 (41 << GPIO_PIN_SHIFT)
#define GPIO_PIN42 (42 << GPIO_PIN_SHIFT)
#define GPIO_PIN43 (43 << GPIO_PIN_SHIFT)
#define GPIO_PIN44 (44 << GPIO_PIN_SHIFT)
#define GPIO_PIN45 (45 << GPIO_PIN_SHIFT)
#define GPIO_PIN46 (46 << GPIO_PIN_SHIFT)
#define GPIO_PIN47 (47 << GPIO_PIN_SHIFT)
#define GPIO_PIN48 (48 << GPIO_PIN_SHIFT)
#define GPIO_PIN49 (49 << GPIO_PIN_SHIFT)
#define GPIO_PIN50 (50 << GPIO_PIN_SHIFT)
#define GPIO_PIN51 (51 << GPIO_PIN_SHIFT)
#define GPIO_PIN52 (52 << GPIO_PIN_SHIFT)
#define GPIO_PIN53 (53 << GPIO_PIN_SHIFT)
#define GPIO_PIN54 (54 << GPIO_PIN_SHIFT)
using namespace DriverFramework;
namespace navio_gpio
{
class Gpio
{
public:
Gpio()
{ }
~Gpio()
{ }
int start();
int stop();
int configgpio(uint32_t pinset);
int unconfiggpio(uint32_t pinset);
bool gpioread(uint32_t pinset);
void gpiowrite(uint32_t pinset, bool value);
bool isMapped() { return _gpio_map != nullptr; }
private:
void atomic_modify(uintptr_t addr,
unsigned int shift,
unsigned int mask,
unsigned int value);
void *_gpio_map;
SyncObj m_lock;
};
} // namespace navio_gpio

44
src/drivers/navio_gpio/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_gpio_test
main.cpp
test.cpp
)
target_link_libraries(navio_gpio_test
drivers__navio_gpio
platforms__posix__px4_layer
df_driver_framework
)

95
src/drivers/navio_gpio/test/test.cpp
View File

@ -1,95 +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 "navio_gpio.h"
#include <stdio.h>
#include <unistd.h>
#define LED_CNF (GPIO_CNF_OUTPUT)
#define LED_pinR GPIO_PIN4
#define LED_pinG GPIO_PIN27
#define LED_pinB GPIO_PIN6
#define LED_OFF 1
#define LED_ON 0
using namespace navio_gpio;
int do_test();
int do_test()
{
Gpio gpio;
if (gpio.start() < 0) {
return -1;
}
gpio.configgpio(LED_CNF | LED_pinR);
gpio.configgpio(LED_CNF | LED_pinG);
gpio.configgpio(LED_CNF | LED_pinB);
gpio.gpiowrite(LED_pinR, LED_OFF);
gpio.gpiowrite(LED_pinG, LED_OFF);
gpio.gpiowrite(LED_pinB, LED_OFF);
printf("off\n");
sleep(2);
gpio.gpiowrite(LED_pinR, LED_ON);
gpio.gpiowrite(LED_pinG, LED_OFF);
gpio.gpiowrite(LED_pinB, LED_OFF);
printf("red\n");
sleep(2);
gpio.gpiowrite(LED_pinR, LED_OFF);
gpio.gpiowrite(LED_pinG, LED_ON);
gpio.gpiowrite(LED_pinB, LED_OFF);
printf("green\n");
sleep(2);
gpio.gpiowrite(LED_pinR, LED_OFF);
gpio.gpiowrite(LED_pinG, LED_OFF);
gpio.gpiowrite(LED_pinB, LED_ON);
printf("blue\n");
sleep(2);
gpio.gpiowrite(LED_pinR, LED_OFF);
gpio.gpiowrite(LED_pinG, LED_OFF);
gpio.gpiowrite(LED_pinB, LED_OFF);
printf("off\n");
gpio.stop();
return 0;
}

2
src/drivers/navio_rgbled/CMakeLists.txt
View File

@ -40,7 +40,7 @@ px4_add_module(
navio_rgbled.cpp
DEPENDS
platforms__common
drivers__navio_gpio
drivers__linux_gpio
lib__led
)

117
src/drivers/navio_rgbled/navio_rgbled.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* 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
@ -36,17 +36,8 @@
#include "navio_rgbled.h"
#define GPIO_LED_CNF (GPIO_CNF_OUTPUT)
#define GPIO_LED_R (GPIO_PIN4)
#define GPIO_LED_G (GPIO_PIN27)
#define GPIO_LED_B (GPIO_PIN6)
#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
// inverted
#define LED_ON 0
#define LED_OFF 1
using namespace DriverFramework;
@ -56,6 +47,9 @@ RGBLED::RGBLED(const char *name)
RGBLED_BASE_DEVICE_PATH,
DeviceBusType_UNKNOWN,
0)
, _gpioR(4)
, _gpioG(27)
, _gpioB(6)
{
};
@ -75,11 +69,47 @@ int RGBLED::start()
return res;
}
_gpio.start();
res = _gpioR.exportPin();
_gpio.configgpio(GPIO_LED_CNF | GPIO_LED_R);
_gpio.configgpio(GPIO_LED_CNF | GPIO_LED_G);
_gpio.configgpio(GPIO_LED_CNF | GPIO_LED_B);
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());
@ -91,6 +121,13 @@ int RGBLED::start()
return res;
}
return res;
cleanup:
_gpioR.unexportPin();
_gpioG.unexportPin();
_gpioB.unexportPin();
return res;
}
@ -98,7 +135,9 @@ int RGBLED::stop()
{
int res;
_gpio.stop();
_gpioR.unexportPin();
_gpioG.unexportPin();
_gpioB.unexportPin();
res = DevObj::stop();
@ -123,52 +162,52 @@ void RGBLED::_measure()
if (_led_controller.update(led_control_data) == 1) {
switch (led_control_data.leds[0].color) {
case led_control_s::COLOR_RED:
_gpio.gpiowrite(GPIO_LED_R, LED_ON);
_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
case led_control_s::COLOR_GREEN:
_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
_gpio.gpiowrite(GPIO_LED_G, LED_ON);
_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
case led_control_s::COLOR_BLUE:
_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
_gpio.gpiowrite(GPIO_LED_B, LED_ON);
_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:
_gpio.gpiowrite(GPIO_LED_R, LED_ON);
_gpio.gpiowrite(GPIO_LED_G, LED_ON);
_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
case led_control_s::COLOR_PURPLE:
_gpio.gpiowrite(GPIO_LED_R, LED_ON);
_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
_gpio.gpiowrite(GPIO_LED_B, LED_ON);
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
case led_control_s::COLOR_CYAN:
_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
_gpio.gpiowrite(GPIO_LED_G, LED_ON);
_gpio.gpiowrite(GPIO_LED_B, LED_ON);
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
case led_control_s::COLOR_WHITE:
_gpio.gpiowrite(GPIO_LED_R, LED_ON);
_gpio.gpiowrite(GPIO_LED_G, LED_ON);
_gpio.gpiowrite(GPIO_LED_B, LED_ON);
_gpioR.writeValue(LinuxGPIO::Value::LOW);
_gpioG.writeValue(LinuxGPIO::Value::LOW);
_gpioB.writeValue(LinuxGPIO::Value::LOW);
break;
default: // led_control_s::COLOR_OFF
_gpio.gpiowrite(GPIO_LED_R, LED_OFF);
_gpio.gpiowrite(GPIO_LED_G, LED_OFF);
_gpio.gpiowrite(GPIO_LED_B, LED_OFF);
_gpioR.writeValue(LinuxGPIO::Value::HIGH);
_gpioG.writeValue(LinuxGPIO::Value::HIGH);
_gpioB.writeValue(LinuxGPIO::Value::HIGH);
break;
}
}

14
src/drivers/navio_rgbled/navio_rgbled.h
View File

@ -32,16 +32,12 @@
****************************************************************************/
#pragma once
#include "DevObj.hpp"
#include "navio_gpio.h"
#include <drivers/linux_gpio/linux_gpio.h>
#include <DevObj.hpp>
#include <lib/led/led.h>
using namespace navio_gpio;
class RGBLED : public DevObj
class RGBLED : public DriverFramework::DevObj
{
public:
RGBLED(const char *name);
@ -55,5 +51,7 @@ protected:
private:
LedController _led_controller;
Gpio _gpio;
LinuxGPIO _gpioR;
LinuxGPIO _gpioG;
LinuxGPIO _gpioB;
};