mirror of https://github.com/ArduPilot/ardupilot
HAL_PX4: added an RC Output driver
This commit is contained in:
parent
4fe7ad6267
commit
0fcc6d7389
|
@ -8,6 +8,7 @@ namespace PX4 {
|
|||
class PX4UARTDriver;
|
||||
class PX4EEPROMStorage;
|
||||
class PX4RCInput;
|
||||
class PX4RCOutput;
|
||||
}
|
||||
|
||||
#endif //__AP_HAL_PX4_NAMESPACE_H__
|
||||
|
|
|
@ -12,6 +12,7 @@
|
|||
#include "UARTDriver.h"
|
||||
#include "Storage.h"
|
||||
#include "RCInput.h"
|
||||
#include "RCOutput.h"
|
||||
|
||||
#include <AP_HAL_Empty.h>
|
||||
#include <AP_HAL_Empty_Private.h>
|
||||
|
@ -29,13 +30,13 @@ static Empty::EmptyI2CDriver i2cDriver;
|
|||
static Empty::EmptySPIDeviceManager spiDeviceManager;
|
||||
static Empty::EmptyAnalogIn analogIn;
|
||||
static Empty::EmptyGPIO gpioDriver;
|
||||
static Empty::EmptyRCOutput rcoutDriver;
|
||||
static Empty::EmptyUtil utilInstance;
|
||||
|
||||
static PX4ConsoleDriver consoleDriver;
|
||||
static PX4Scheduler schedulerInstance;
|
||||
static PX4EEPROMStorage storageDriver;
|
||||
static PX4RCInput rcinDriver;
|
||||
static PX4RCOutput rcoutDriver;
|
||||
|
||||
#define UARTA_DEVICE "/dev/ttyS2"
|
||||
#define UARTB_DEVICE "/dev/ttyS3"
|
||||
|
@ -72,10 +73,12 @@ static int main_loop(int argc, char **argv)
|
|||
{
|
||||
extern void setup(void);
|
||||
extern void loop(void);
|
||||
|
||||
hal.uartA->begin(115200);
|
||||
hal.console->init((void*) hal.uartA);
|
||||
hal.scheduler->init(NULL);
|
||||
hal.rcin->init(NULL);
|
||||
hal.rcout->init(NULL);
|
||||
|
||||
setup();
|
||||
|
||||
|
|
|
@ -0,0 +1,86 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
#include "RCOutput.h"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace PX4;
|
||||
|
||||
void PX4RCOutput::init(void* unused)
|
||||
{
|
||||
_pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
|
||||
if (_pwm_fd == -1) {
|
||||
hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
|
||||
}
|
||||
ioctl(_pwm_fd, PWM_SERVO_ARM, 0);
|
||||
}
|
||||
|
||||
void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
||||
{
|
||||
// no support for this yet
|
||||
}
|
||||
|
||||
uint16_t PX4RCOutput::get_freq(uint8_t ch)
|
||||
{
|
||||
return 50;
|
||||
}
|
||||
|
||||
void PX4RCOutput::enable_ch(uint8_t ch)
|
||||
{
|
||||
// channels are always enabled ...
|
||||
}
|
||||
|
||||
void PX4RCOutput::enable_mask(uint32_t chmask)
|
||||
{
|
||||
// channels are always enabled ...
|
||||
}
|
||||
|
||||
void PX4RCOutput::disable_ch(uint8_t ch)
|
||||
{
|
||||
// channels are always enabled ...
|
||||
}
|
||||
|
||||
void PX4RCOutput::disable_mask(uint32_t chmask)
|
||||
{
|
||||
// channels are always enabled ...
|
||||
}
|
||||
|
||||
void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
|
||||
{
|
||||
ioctl(_pwm_fd, PWM_SERVO_SET(ch), (unsigned long)period_us);
|
||||
}
|
||||
|
||||
void PX4RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
|
||||
{
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
write(i, period_us[i]);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t PX4RCOutput::read(uint8_t ch)
|
||||
{
|
||||
servo_position_t pos;
|
||||
if (ioctl(_pwm_fd, PWM_SERVO_GET(ch), (unsigned long)&pos) == 0) {
|
||||
return pos;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
|
||||
{
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
period_us[i] = read(i);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_HAL_BOARD
|
|
@ -0,0 +1,26 @@
|
|||
|
||||
#ifndef __AP_HAL_PX4_RCOUTPUT_H__
|
||||
#define __AP_HAL_PX4_RCOUTPUT_H__
|
||||
|
||||
#include <AP_HAL_PX4.h>
|
||||
|
||||
class PX4::PX4RCOutput : public AP_HAL::RCOutput
|
||||
{
|
||||
public:
|
||||
void init(void* machtnichts);
|
||||
void set_freq(uint32_t chmask, uint16_t freq_hz);
|
||||
uint16_t get_freq(uint8_t ch);
|
||||
void enable_ch(uint8_t ch);
|
||||
void enable_mask(uint32_t chmask);
|
||||
void disable_ch(uint8_t ch);
|
||||
void disable_mask(uint32_t chmask);
|
||||
void write(uint8_t ch, uint16_t period_us);
|
||||
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
|
||||
uint16_t read(uint8_t ch);
|
||||
void read(uint16_t* period_us, uint8_t len);
|
||||
|
||||
private:
|
||||
int _pwm_fd;
|
||||
};
|
||||
|
||||
#endif // __AP_HAL_PX4_RCOUTPUT_H__
|
Loading…
Reference in New Issue