PX4 Firmware
PX4 Autopilot Software http://px4.io
PCA9685.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  *
33  * Original Author : Georgi Todorov
34  * Edited by : Tord Wessman
35  * Created on : Nov 22, 2013
36  * Rewrite : Fan.zhang 421395590@qq.com
37  * Updated on : Mar 2, 2017
38  ****************************************************************************/
39 
40 #pragma once
41 #include <inttypes.h>
42 #include "common.h"
43 
44 namespace linux_pwm_out
45 {
46 
47 // Register Definitions
48 // 寄存器定义
49 #define MODE1 0x00 //Mode register 1
50 #define MODE2 0x01 //Mode register 2
51 #define SUBADR1 0x02 //I2C-bus subaddress 1
52 #define SUBADR2 0x03 //I2C-bus subaddress 2
53 #define SUBADR3 0x04 //I2C-bus subaddress 3
54 #define ALLCALLADR 0x05 //LED All Call I2C-bus address
55 #define LED0 0x6 //LED0 start register
56 #define LED0_ON_L 0x6 //LED0 output and brightness control byte 0
57 #define LED0_ON_H 0x7 //LED0 output and brightness control byte 1
58 #define LED0_OFF_L 0x8 //LED0 output and brightness control byte 2
59 #define LED0_OFF_H 0x9 //LED0 output and brightness control byte 3
60 #define LED_MULTIPLYER 4 // For the other 15 channels
61 #define ALLLED_ON_L 0xFA //load all the LEDn_ON registers, byte 0 (turn 0-7 channels on)
62 #define ALLLED_ON_H 0xFB //load all the LEDn_ON registers, byte 1 (turn 8-15 channels on)
63 #define ALLLED_OFF_L 0xFC //load all the LEDn_OFF registers, byte 0 (turn 0-7 channels off)
64 #define ALLLED_OFF_H 0xFD //load all the LEDn_OFF registers, byte 1 (turn 8-15 channels off)
65 #define PRE_SCALE 0xFE //prescaler for output frequency
66 #define MAX_PWM_RES 4096 //Resolution 4096=12bit 分辨率,按2的阶乘计算,12bit为4096
67 #define CLOCK_FREQ 25000000.0 //25MHz default osc clock
68 #define PCA9685_DEFAULT_I2C_ADDR 0x40 // default i2c address for pca9685 默认i2c地址为0x40
69 #define PCA9685_DEFAULT_I2C_BUS 1 // default i2c bus for pca9685 默认为1
70 
71 //! Main class that exports features for PCA9685 chip
72 class PCA9685 : public PWMOutBase
73 {
74 public:
75 
76  PCA9685();
77 
78  /**
79  * Constructor takes bus and address arguments
80  * @param bus the bus to use in /dev/i2c-%d.
81  * @param address the device address on bus
82  */
83  PCA9685(int bus, int address);
84 
85  int init() override { return _fd >= 0 ? 0 : -1; }
86 
87  int send_output_pwm(const uint16_t *pwm, int num_outputs) override;
88 
89 
90  int init(int bus, int address);
91  virtual ~PCA9685();
92 
93  /** Sets PCA9685 mode to 00 */
94  void reset();
95 
96  /**
97  * Set the frequency of PWM
98  * @param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator.
99  */
100  void setPWMFreq(int freq);
101 
102  /**
103  * PWM a single channel with custom on time.
104  * send pwm vale to led(channel)
105  * @param channel
106  * @param on_value 0-4095 value to turn on the pulse
107  * @param off_value 0-4095 value to turn off the pulse
108  */
109  void setPWM(uint8_t channel, int on_value, int off_value);
110 
111  /**
112  * send pwm value to led(channel),
113  * value should be range of 0-4095
114  */
115  void setPWM(uint8_t channel, int value);
116 
117 private:
118  int _fd = -1; ///< I2C device file descriptor
119 
120  /**
121  * Read a single byte from PCA9685
122  * @param fd file descriptor for I/O
123  * @param address register address to read from
124  * @return read byte
125  */
126  uint8_t read_byte(int fd, uint8_t address);
127 
128  /**
129  * Write a single byte to PCA9685
130  * @param fd file descriptor for I/O
131  * @param address register address to write to
132  * @param data 8 bit data to write
133  */
134  void write_byte(int fd, uint8_t address, uint8_t data);
135 
136  /**
137  * Open device file for PCA9685 I2C bus
138  * @return fd returns the file descriptor number or -1 on error
139  */
140  int open_fd(int bus, int address);
141 };
142 
143 }
void reset()
Sets PCA9685 mode to 00.
Definition: PCA9685.cpp:103
Main class that exports features for PCA9685 chip.
Definition: PCA9685.h:72
uint8_t read_byte(int fd, uint8_t address)
Read a single byte from PCA9685.
Definition: PCA9685.cpp:148
void setPWMFreq(int freq)
Set the frequency of PWM.
Definition: PCA9685.cpp:111
void write_byte(int fd, uint8_t address, uint8_t data)
Write a single byte to PCA9685.
Definition: PCA9685.cpp:165
int init() override
Definition: PCA9685.h:85
int _fd
I2C device file descriptor.
Definition: PCA9685.h:118
uint8_t * data
Definition: dataman.cpp:149
class PWMOutBase common abstract PWM output base class
Definition: common.h:45
int fd
Definition: dataman.cpp:146
int send_output_pwm(const uint16_t *pwm, int num_outputs) override
Definition: PCA9685.cpp:71
int open_fd(int bus, int address)
Open device file for PCA9685 I2C bus.
Definition: PCA9685.cpp:176
void setPWM(uint8_t channel, int on_value, int off_value)
PWM a single channel with custom on time.
Definition: PCA9685.cpp:134