ardupilot/libraries/AP_HAL_VRBRAIN/GPIO.cpp

242 lines
5.4 KiB
C++

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "GPIO.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
/* VRBRAIN headers */
#include <drivers/drv_led.h>
#include <drivers/drv_buzzer.h>
#include <drivers/drv_gpio.h>
#include <arch/board/board.h>
#include <board_config.h>
#define LOW 0
#define HIGH 1
extern const AP_HAL::HAL& hal;
using namespace VRBRAIN;
VRBRAINGPIO::VRBRAINGPIO()
{}
void VRBRAINGPIO::init()
{
_led_fd = open(LED_DEVICE_PATH, O_RDWR);
if (_led_fd == -1) {
hal.scheduler->panic("Unable to open " LED_DEVICE_PATH);
}
if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n");
}
if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED RED\n");
}
if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n");
}
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
if (ioctl(_led_fd, LED_OFF, LED_EXT1) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 1\n");
}
#endif
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
if (ioctl(_led_fd, LED_OFF, LED_EXT2) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 2\n");
}
#endif
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
if (ioctl(_led_fd, LED_OFF, LED_EXT3) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 3\n");
}
#endif
_buzzer_fd = open(BUZZER_DEVICE_PATH, O_RDWR);
if (_buzzer_fd == -1) {
hal.scheduler->panic("Unable to open " BUZZER_DEVICE_PATH);
}
if (ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO BUZZER\n");
}
}
void VRBRAINGPIO::pinMode(uint8_t pin, uint8_t output)
{
switch (pin) {
}
}
int8_t VRBRAINGPIO::analogPinToDigitalPin(uint8_t pin)
{
return -1;
}
uint8_t VRBRAINGPIO::read(uint8_t pin)
{
switch (pin) {
}
return LOW;
}
void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
{
switch (pin) {
case HAL_GPIO_A_LED_PIN: // Arming LED
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_GREEN);
} else {
ioctl(_led_fd, LED_ON, LED_GREEN);
}
break;
case HAL_GPIO_B_LED_PIN: // not used yet
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_BLUE);
} else {
ioctl(_led_fd, LED_ON, LED_BLUE);
}
break;
case HAL_GPIO_C_LED_PIN: // GPS LED
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_RED);
} else {
ioctl(_led_fd, LED_ON, LED_RED);
}
break;
case EXTERNAL_LED_GPS:
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_EXT1);
} else {
ioctl(_led_fd, LED_ON, LED_EXT1);
}
#endif
break;
case EXTERNAL_LED_ARMED:
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_EXT2);
} else {
ioctl(_led_fd, LED_ON, LED_EXT2);
}
#endif
break;
case EXTERNAL_LED_MOTOR1:
break;
case EXTERNAL_LED_MOTOR2:
break;
case BUZZER_PIN:
if (value == LOW) {
ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT);
} else {
ioctl(_buzzer_fd, BUZZER_ON, BUZZER_EXT);
}
break;
}
}
void VRBRAINGPIO::toggle(uint8_t pin)
{
switch (pin) {
case HAL_GPIO_A_LED_PIN: // Arming LED
ioctl(_led_fd, LED_TOGGLE, LED_GREEN);
break;
case HAL_GPIO_B_LED_PIN: // not used yet
ioctl(_led_fd, LED_TOGGLE, LED_BLUE);
break;
case HAL_GPIO_C_LED_PIN: // GPS LED
ioctl(_led_fd, LED_TOGGLE, LED_RED);
break;
case EXTERNAL_LED_GPS:
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
ioctl(_led_fd, LED_TOGGLE, LED_EXT1);
#endif
break;
case EXTERNAL_LED_ARMED:
#if !defined(CONFIG_ARCH_BOARD_VRHERO_V10)
ioctl(_led_fd, LED_TOGGLE, LED_EXT2);
#endif
break;
case EXTERNAL_LED_MOTOR1:
break;
case EXTERNAL_LED_MOTOR2:
break;
case BUZZER_PIN:
ioctl(_buzzer_fd, BUZZER_TOGGLE, BUZZER_EXT);
break;
default:
write(pin, !read(pin));
break;
}
}
/* Alternative interface: */
AP_HAL::DigitalSource* VRBRAINGPIO::channel(uint16_t n) {
return new VRBRAINDigitalSource(0);
}
/* Interrupt interface: */
bool VRBRAINGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode)
{
return true;
}
/*
return true when USB connected
*/
bool VRBRAINGPIO::usb_connected(void)
{
return stm32_gpioread(GPIO_OTGFS_VBUS);
}
VRBRAINDigitalSource::VRBRAINDigitalSource(uint8_t v) :
_v(v)
{}
void VRBRAINDigitalSource::mode(uint8_t output)
{}
uint8_t VRBRAINDigitalSource::read() {
return _v;
}
void VRBRAINDigitalSource::write(uint8_t value) {
_v = value;
}
void VRBRAINDigitalSource::toggle() {
_v = !_v;
}
#endif // CONFIG_HAL_BOARD