zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

RPI PCA9685: code cleanup

sbg
Beat Küng 8 years ago
parent
d898b555d3
commit
8ccbc068c2
3 changed files with 111 additions and 113 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 150
      src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp
  2. 70
      src/drivers/rpi_pca9685_pwm_out/PCA9685.h
  3. 4
      src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h

150
src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -43,176 +43,128 @@
#include <linux/i2c-dev.h>
#include <stdio.h> /* Standard I/O functions */
#include <fcntl.h>
#include <syslog.h> /* Syslog functionallity */
#include <inttypes.h>
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <unistd.h>
//
#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 <px4_log.h>
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();
if (_fd >= 0) {
close(_fd);
}
}
//! 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 != -1) {
write_byte(_fd, MODE1, 0x00); //Normal mode
write_byte(_fd, MODE2, 0x04); //Normal mode
}
}
//! Set the frequency of PWM
/*!
\param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator.
*/
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 (write(fd, buf, 1) != 1) {
return -1;
}
if (read(fd, _dataBuffer, BUFFER_SIZE) != BUFFER_SIZE) {
return (-1);
}*/
if (read(fd, buf, 1) != 1) {
return -1;
}
return buf[0];
}
//! 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;
uint8_t buf[2];
buf[0] = address;
buf[1] = data;
if (write(fd, buff, sizeof(buff)) != 2) {
usleep(5000);
if (write(fd, buf, sizeof(buf)) != sizeof(buf)) {
PX4_ERR("Write failed (%i)", errno);
}
}
//! Open device file for PCA9685 I2C bus
/*!
\return fd returns the file descriptor number or -1 on error
*/
int PCA9685::openfd()
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(_busfile, O_RDWR)) < 0) {
//printf("Couldn't open I2C Bus %d [openfd():open %d]", _i2cbus, errno);
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, _i2caddr) < 0) {
//printf("I2C slave %d failed [openfd():ioctl %d]", _i2caddr, errno);
if (ioctl(fd, I2C_SLAVE, address) < 0) {
PX4_ERR("I2C slave %d failed [open_fd():ioctl %d]", address, errno);
return -1;
}

70
src/drivers/rpi_pca9685_pwm_out/PCA9685.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -37,8 +37,7 @@
* Updated on : Mar 2, 2017
****************************************************************************/
#ifndef _PCA9685_H
#define _PCA9685_H
#pragma once
#include <inttypes.h>
// Register Definitions
@ -62,29 +61,72 @@ @@ -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

4
src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h
Unescape View File

@ -30,8 +30,12 @@ @@ -30,8 +30,12 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include <px4_tasks.h>
#include <px4_getopt.h>
#include <px4_posix.h>

Write Preview
Loading…
Cancel
Save