You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
138 lines
3.7 KiB
138 lines
3.7 KiB
|
|
#include <AP_HAL.h> |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
|
|
|
#include "RCOutput_Navio.h" |
|
#include <sys/types.h> |
|
#include <sys/stat.h> |
|
#include <fcntl.h> |
|
#include <unistd.h> |
|
#include <dirent.h> |
|
#include <stdlib.h> |
|
#include <stdio.h> |
|
#include <stdint.h> |
|
#include <math.h> |
|
|
|
using namespace Linux; |
|
|
|
#define PWM_CHAN_COUNT 13 |
|
|
|
static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; |
|
|
|
void LinuxRCOutput_Navio::init(void* machtnicht) |
|
{ |
|
_i2c_sem = hal.i2c->get_semaphore(); |
|
if (_i2c_sem == NULL) { |
|
hal.scheduler->panic(PSTR("PANIC: RCOutput_Navio did not get " |
|
"valid I2C semaphore!")); |
|
return; // never reached |
|
} |
|
|
|
// Set the initial frequency |
|
set_freq(0, 50); |
|
} |
|
|
|
void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz) |
|
{ |
|
if (!_i2c_sem->take(10)) { |
|
return; |
|
} |
|
|
|
// Put PCA9685 to sleep (required to write prescaler) |
|
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT); |
|
|
|
// Calculate and write prescale value to match frequency |
|
uint8_t prescale = round(24576000.f / 4096.f / freq_hz) - 1; |
|
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale); |
|
|
|
// Reset all channels |
|
uint8_t data[4] = {0x00, 0x00, 0x00, 0x00}; |
|
hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_RA_ALL_LED_ON_L, 4, data); |
|
|
|
// Enable external clocking |
|
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, |
|
PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT); |
|
|
|
// Restart the device to apply new settings and enable auto-incremented write |
|
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, |
|
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT); |
|
_frequency = freq_hz; |
|
|
|
_i2c_sem->give(); |
|
} |
|
|
|
uint16_t LinuxRCOutput_Navio::get_freq(uint8_t ch) |
|
{ |
|
return _frequency; |
|
} |
|
|
|
void LinuxRCOutput_Navio::enable_ch(uint8_t ch) |
|
{ |
|
|
|
} |
|
|
|
void LinuxRCOutput_Navio::disable_ch(uint8_t ch) |
|
{ |
|
write(ch, 0); |
|
} |
|
|
|
void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us) |
|
{ |
|
if(ch >= PWM_CHAN_COUNT){ |
|
return; |
|
} |
|
|
|
if (!_i2c_sem->take_nonblocking()) { |
|
return; |
|
} |
|
|
|
uint16_t length; |
|
|
|
if (period_us == 0) |
|
length = 0; |
|
else |
|
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1; |
|
|
|
uint8_t data[2] = {length & 0xFF, length >> 8}; |
|
uint8_t status = hal.i2c->writeRegisters(PCA9685_ADDRESS, |
|
PCA9685_RA_LED0_OFF_L + 4 * (ch + 3), |
|
2, |
|
data); |
|
|
|
_i2c_sem->give(); |
|
} |
|
|
|
void LinuxRCOutput_Navio::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) |
|
{ |
|
if (!_i2c_sem->take_nonblocking()) { |
|
return 0; |
|
} |
|
|
|
uint8_t data[4] = {0x00, 0x00, 0x00, 0x00}; |
|
hal.i2c->readRegisters(PCA9685_ADDRESS, |
|
PCA9685_RA_LED0_ON_L + 4 * (ch + 3), |
|
4, |
|
data); |
|
|
|
uint16_t length = data[2] + ((data[3] & 0x0F) << 8); |
|
uint16_t period_us = (length + 1) * (1000000.f / _frequency) / 4096.f; |
|
|
|
_i2c_sem->give(); |
|
|
|
return length == 0 ? 0 : period_us; |
|
} |
|
|
|
void LinuxRCOutput_Navio::read(uint16_t* period_us, uint8_t len) |
|
{ |
|
for (int i = 0; i < len; i++) |
|
period_us[i] = read(0 + i); |
|
} |
|
|
|
#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
|
|
|