|
|
|
@ -4,7 +4,7 @@
@@ -4,7 +4,7 @@
|
|
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
|
|
|
|
|
|
|
|
|
#include "RCOutput_Navio.h" |
|
|
|
|
#include "RCOutput_PCA9685.h" |
|
|
|
|
#include <sys/types.h> |
|
|
|
|
#include <sys/stat.h> |
|
|
|
|
#include <fcntl.h> |
|
|
|
@ -58,9 +58,9 @@ using namespace Linux;
@@ -58,9 +58,9 @@ using namespace Linux;
|
|
|
|
|
|
|
|
|
|
static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; |
|
|
|
|
|
|
|
|
|
LinuxRCOutput_Navio::LinuxRCOutput_Navio(bool external_clock, |
|
|
|
|
uint8_t channel_offset, |
|
|
|
|
uint8_t oe_pin_number) : |
|
|
|
|
LinuxRCOutput_PCA9685::LinuxRCOutput_PCA9685(bool external_clock, |
|
|
|
|
uint8_t channel_offset, |
|
|
|
|
uint8_t oe_pin_number) : |
|
|
|
|
_i2c_sem(NULL), |
|
|
|
|
_enable_pin(NULL), |
|
|
|
|
_frequency(50), |
|
|
|
@ -75,16 +75,16 @@ LinuxRCOutput_Navio::LinuxRCOutput_Navio(bool external_clock,
@@ -75,16 +75,16 @@ LinuxRCOutput_Navio::LinuxRCOutput_Navio(bool external_clock,
|
|
|
|
|
_osc_clock = PCA9685_INTERNAL_CLOCK; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
LinuxRCOutput_Navio::~LinuxRCOutput_Navio() |
|
|
|
|
LinuxRCOutput_PCA9685::~LinuxRCOutput_PCA9685() |
|
|
|
|
{ |
|
|
|
|
delete [] _pulses_buffer; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::init(void* machtnicht) |
|
|
|
|
void LinuxRCOutput_PCA9685::init(void* machtnicht) |
|
|
|
|
{ |
|
|
|
|
_i2c_sem = hal.i2c->get_semaphore(); |
|
|
|
|
if (_i2c_sem == NULL) { |
|
|
|
|
hal.scheduler->panic(PSTR("PANIC: RCOutput_Navio did not get " |
|
|
|
|
hal.scheduler->panic(PSTR("PANIC: RCOutput_PCA9685 did not get " |
|
|
|
|
"valid I2C semaphore!")); |
|
|
|
|
return; /* never reached */ |
|
|
|
|
} |
|
|
|
@ -100,7 +100,7 @@ void LinuxRCOutput_Navio::init(void* machtnicht)
@@ -100,7 +100,7 @@ void LinuxRCOutput_Navio::init(void* machtnicht)
|
|
|
|
|
_enable_pin->write(0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::reset_all_channels() |
|
|
|
|
void LinuxRCOutput_PCA9685::reset_all_channels() |
|
|
|
|
{ |
|
|
|
|
if (!_i2c_sem->take(10)) { |
|
|
|
|
return; |
|
|
|
@ -115,7 +115,7 @@ void LinuxRCOutput_Navio::reset_all_channels()
@@ -115,7 +115,7 @@ void LinuxRCOutput_Navio::reset_all_channels()
|
|
|
|
|
_i2c_sem->give(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz) |
|
|
|
|
void LinuxRCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
/* Correctly finish last pulses */ |
|
|
|
@ -158,22 +158,22 @@ void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz)
@@ -158,22 +158,22 @@ void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz)
|
|
|
|
|
_i2c_sem->give(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint16_t LinuxRCOutput_Navio::get_freq(uint8_t ch) |
|
|
|
|
uint16_t LinuxRCOutput_PCA9685::get_freq(uint8_t ch) |
|
|
|
|
{ |
|
|
|
|
return _frequency; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::enable_ch(uint8_t ch) |
|
|
|
|
void LinuxRCOutput_PCA9685::enable_ch(uint8_t ch) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::disable_ch(uint8_t ch) |
|
|
|
|
void LinuxRCOutput_PCA9685::disable_ch(uint8_t ch) |
|
|
|
|
{ |
|
|
|
|
write(ch, 0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us) |
|
|
|
|
void LinuxRCOutput_PCA9685::write(uint8_t ch, uint16_t period_us) |
|
|
|
|
{ |
|
|
|
|
if(ch >= PWM_CHAN_COUNT){ |
|
|
|
|
return; |
|
|
|
@ -201,18 +201,18 @@ void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us)
@@ -201,18 +201,18 @@ void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us)
|
|
|
|
|
_i2c_sem->give(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t* period_us, uint8_t len) |
|
|
|
|
void LinuxRCOutput_PCA9685::write(uint8_t ch, uint16_t* period_us, uint8_t len) |
|
|
|
|
{ |
|
|
|
|
for (int i = 0; i < len; i++) |
|
|
|
|
write(ch + i, period_us[i]); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
uint16_t LinuxRCOutput_Navio::read(uint8_t ch) |
|
|
|
|
uint16_t LinuxRCOutput_PCA9685::read(uint8_t ch) |
|
|
|
|
{ |
|
|
|
|
return _pulses_buffer[ch]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LinuxRCOutput_Navio::read(uint16_t* period_us, uint8_t len) |
|
|
|
|
void LinuxRCOutput_PCA9685::read(uint16_t* period_us, uint8_t len) |
|
|
|
|
{ |
|
|
|
|
for (int i = 0; i < len; i++)
|
|
|
|
|
period_us[i] = read(0 + i); |