diff --git a/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp b/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp index f484d621df..3cbaf1562e 100644 --- a/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,176 +43,128 @@ #include #include /* Standard I/O functions */ #include -#include /* Syslog functionallity */ #include #include #include #include #include -// #include "PCA9685.h" -//! Constructor takes bus and address arguments -/*! - \param bus the bus to use in /dev/i2c-%d. - \param address the device address on bus - */ +#include void PCA9685::init(int bus, int address) { - _i2cbus = bus; - _i2caddr = address; - snprintf(_busfile, sizeof(_busfile), "/dev/i2c-%d", bus); + _fd = open_fd(bus, address); reset(); - //usleep(10*1000); } -PCA9685::PCA9685() : - _i2caddr(PCA9685_DEFAULT_I2C_ADDR), - _i2cbus(PCA9685_DEFAULT_I2C_BUS), - _dataBuffer {} +PCA9685::PCA9685() { - + init(PCA9685_DEFAULT_I2C_BUS, PCA9685_DEFAULT_I2C_ADDR); } -PCA9685::PCA9685(int bus, int address) : - _busfile {}, - _dataBuffer {} +PCA9685::PCA9685(int bus, int address) { - _i2cbus = bus; - _i2caddr = address; - snprintf(_busfile, sizeof(_busfile), "/dev/i2c-%d", bus); - reset(); + init(bus, address); } PCA9685::~PCA9685() { reset(); -} -//! Sets PCA9685 mode to 00 -void PCA9685::reset() -{ - int fd = openfd(); - if (fd != -1) { - write_byte(fd, MODE1, 0x00); //Normal mode - write_byte(fd, MODE2, 0x04); //Normal mode - close(fd); + if (_fd >= 0) { + close(_fd); } } -//! Set the frequency of PWM -/*! - \param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator. - */ + +void PCA9685::reset() +{ + if (_fd != -1) { + write_byte(_fd, MODE1, 0x00); //Normal mode + write_byte(_fd, MODE2, 0x04); //Normal mode + } +} + void PCA9685::setPWMFreq(int freq) { - int fd = openfd(); - - if (fd != -1) { + if (_fd != -1) { uint8_t prescale = (CLOCK_FREQ / MAX_PWM_RES / freq) - 1; //printf ("Setting prescale value to: %d\n", prescale); //printf ("Using Frequency: %d\n", freq); - uint8_t oldmode = read_byte(fd, MODE1); + uint8_t oldmode = read_byte(_fd, MODE1); uint8_t newmode = (oldmode & 0x7F) | 0x10; //sleep - write_byte(fd, MODE1, newmode); // go to sleep - write_byte(fd, PRE_SCALE, prescale); - write_byte(fd, MODE1, oldmode); + write_byte(_fd, MODE1, newmode); // go to sleep + write_byte(_fd, PRE_SCALE, prescale); + write_byte(_fd, MODE1, oldmode); usleep(10 * 1000); - write_byte(fd, MODE1, oldmode | 0x80); - - close(fd); + write_byte(_fd, MODE1, oldmode | 0x80); } } -/** - *send pwn vale to led(channel), - *value should be range of 0-4095 - */ void PCA9685::setPWM(uint8_t led, int value) { setPWM(led, 0, value); } -//! PWM a single channel with custom on time -/*! - *send pwn vale to led(channel), - *param on_value 0-4095 value to turn on the pulse - *param off_value 0-4095 value to turn off the pulse - */ + void PCA9685::setPWM(uint8_t led, int on_value, int off_value) { - int fd = openfd(); + if (_fd != -1) { + write_byte(_fd, LED0_ON_L + LED_MULTIPLYER * led, on_value & 0xFF); - if (fd != -1) { + write_byte(_fd, LED0_ON_H + LED_MULTIPLYER * led, on_value >> 8); - write_byte(fd, LED0_ON_L + LED_MULTIPLYER * led, on_value & 0xFF); + write_byte(_fd, LED0_OFF_L + LED_MULTIPLYER * led, off_value & 0xFF); - write_byte(fd, LED0_ON_H + LED_MULTIPLYER * led, on_value >> 8); - - write_byte(fd, LED0_OFF_L + LED_MULTIPLYER * led, off_value & 0xFF); - - write_byte(fd, LED0_OFF_H + LED_MULTIPLYER * led, off_value >> 8); - - close(fd); + write_byte(_fd, LED0_OFF_H + LED_MULTIPLYER * led, off_value >> 8); } } -//! Read a single byte from PCA9685 -/*! - \param fd file descriptor for I/O - \param address register address to read from - */ uint8_t PCA9685::read_byte(int fd, uint8_t address) { - return 0; - /* - uint8_t buff[BUFFER_SIZE]; - buff[0] = address; + uint8_t buf[1]; + buf[0] = address; - if (write(fd, buff, BUFFER_SIZE) != BUFFER_SIZE) { - return (-1); - } - - if (read(fd, _dataBuffer, BUFFER_SIZE) != BUFFER_SIZE) { - return (-1); - }*/ -} -//! Write a single byte from PCA9685 -/*! - \param fd file descriptor for I/O - \param address register address to write to - \param data 8 bit data to write - */ -void PCA9685::write_byte(int fd, uint8_t address, uint8_t data) -{ - uint8_t buff[2]; - buff[0] = address; - buff[1] = data; - - if (write(fd, buff, sizeof(buff)) != 2) { - usleep(5000); - } -} -//! Open device file for PCA9685 I2C bus -/*! - \return fd returns the file descriptor number or -1 on error - */ -int PCA9685::openfd() -{ - int fd; - - if ((fd = open(_busfile, O_RDWR)) < 0) { - //printf("Couldn't open I2C Bus %d [openfd():open %d]", _i2cbus, errno); + if (write(fd, buf, 1) != 1) { return -1; } - if (ioctl(fd, I2C_SLAVE, _i2caddr) < 0) { - //printf("I2C slave %d failed [openfd():ioctl %d]", _i2caddr, errno); + if (read(fd, buf, 1) != 1) { + return -1; + } + + return buf[0]; +} + +void PCA9685::write_byte(int fd, uint8_t address, uint8_t data) +{ + uint8_t buf[2]; + buf[0] = address; + buf[1] = data; + + if (write(fd, buf, sizeof(buf)) != sizeof(buf)) { + PX4_ERR("Write failed (%i)", errno); + } +} + +int PCA9685::open_fd(int bus, int address) +{ + int fd; + char bus_file[64]; + snprintf(bus_file, sizeof(bus_file), "/dev/i2c-%d", bus); + + if ((fd = open(bus_file, O_RDWR)) < 0) { + PX4_ERR("Couldn't open I2C Bus %d [open_fd():open %d]", bus, errno); + return -1; + } + + if (ioctl(fd, I2C_SLAVE, address) < 0) { + PX4_ERR("I2C slave %d failed [open_fd():ioctl %d]", address, errno); return -1; } diff --git a/src/drivers/rpi_pca9685_pwm_out/PCA9685.h b/src/drivers/rpi_pca9685_pwm_out/PCA9685.h index 49836afc39..fd915f3143 100644 --- a/src/drivers/rpi_pca9685_pwm_out/PCA9685.h +++ b/src/drivers/rpi_pca9685_pwm_out/PCA9685.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,8 +37,7 @@ * Updated on : Mar 2, 2017 ****************************************************************************/ -#ifndef _PCA9685_H -#define _PCA9685_H +#pragma once #include // Register Definitions @@ -62,29 +61,72 @@ #define PRE_SCALE 0xFE //prescaler for output frequency #define MAX_PWM_RES 4096 //Resolution 4096=12bit 分辨率，按2的阶乘计算，12bit为4096 #define CLOCK_FREQ 25000000.0 //25MHz default osc clock -#define BUFFER_SIZE 0x08 //1 byte buffer #define PCA9685_DEFAULT_I2C_ADDR 0x40 // default i2c address for pca9685 默认i2c地址为0x40 #define PCA9685_DEFAULT_I2C_BUS 1 // default i2c bus for pca9685 默认为1 + //! Main class that exports features for PCA9685 chip class PCA9685 { public: PCA9685(); + + /** + * Constructor takes bus and address arguments + * @param bus the bus to use in /dev/i2c-%d. + * @param address the device address on bus + */ PCA9685(int bus, int address); + void init(int bus, int address); - virtual ~PCA9685(); - void reset(void); + ~PCA9685(); + + /// Sets PCA9685 mode to 00 + void reset(); + + /** + * Set the frequency of PWM + * @param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator. + */ void setPWMFreq(int freq); - void setPWM(uint8_t channel, int on, int off); - void setPWM(uint8_t cahnnel, int off); + + /** + * PWM a single channel with custom on time. + * send pwm vale to led(channel) + * @param channel + * @param on_value 0-4095 value to turn on the pulse + * @param off_value 0-4095 value to turn off the pulse + */ + void setPWM(uint8_t channel, int on_value, int off_value); + + /** + * send pwm value to led(channel), + * value should be range of 0-4095 + */ + void setPWM(uint8_t channel, int value); + private: - int _i2caddr; - int _i2cbus; - char _busfile[64]; - uint8_t _dataBuffer[BUFFER_SIZE]; + int _fd = -1; ///< I2C device file descriptor + + /** + * Read a single byte from PCA9685 + * @param fd file descriptor for I/O + * @param address register address to read from + * @return read byte + */ uint8_t read_byte(int fd, uint8_t address); + + /** + * Write a single byte to PCA9685 + * @param fd file descriptor for I/O + * @param address register address to write to + * @param data 8 bit data to write + */ void write_byte(int fd, uint8_t address, uint8_t data); - int openfd(); + + /** + * Open device file for PCA9685 I2C bus + * @return fd returns the file descriptor number or -1 on error + */ + int open_fd(int bus, int address); }; -#endif diff --git a/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h b/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h index aadd773f62..ffca2efb8a 100644 --- a/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h +++ b/src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h @@ -30,8 +30,12 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + +#pragma once + #include + #include #include #include