ardupilot/libraries/AP_HAL_Linux/RCInput_Navio.cpp

77 lines
1.9 KiB
C++

#include <AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include "RCInput_Navio.h"
extern const AP_HAL::HAL& hal;
using namespace Linux;
// This is a prototype code for RC input decoding on Raspberry Pi.
// It uses pigpio daemon to sample GPIOs over DMA with 1 microsecond resolution.
// This code should be rewritten to configure DMA GPIO sampling without pigpio.
void LinuxRCInput_Navio::init(void*)
{
curtick = 0;
prevtick = 0;
width_s0 = 0;
width_s1 = 0;
// Kills pigpio daemon in case it was run with wrong parameters
system("killall pigpiod -q");
hal.scheduler->delay(1000);
// Starts pigpio daemon with 1 microsecond sampling resolution
system("pigpiod -s 1");
hal.scheduler->delay(1000);
// Configures pigpiod to send GPIO change notifications to /dev/pigpio0
system("pigs NO NB 0 0x10");
hal.scheduler->delay(1000);
// Configures /dev/pigpio0 for non-blocking read
pigpio = open("/dev/pigpio0", O_RDONLY);
if (pigpio == -1)
hal.scheduler->panic("No pigpio interface for RCInput");
}
void LinuxRCInput_Navio::_timer_tick()
{
while (true) {
int bytesAvailable;
ioctl(pigpio, FIONREAD, &bytesAvailable);
if (bytesAvailable >= 12)
::read(pigpio, reinterpret_cast<uint8_t*>(&gpioReport), 12);
else
break;
prevtick = curtick;
curtick = gpioReport.tick;
if (((gpioReport.level >> 4) & 0x01) == 0) {
width_s0 = curtick - prevtick;
}
else {
width_s1 = curtick - prevtick;
_process_rc_pulse(width_s0, width_s1);
}
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE