mirror of
https://github.com/nymea/nymea-plugins.git
synced 2026-07-17 00:36:35 +02:00
93 lines
2.7 KiB
C++
93 lines
2.7 KiB
C++
#include "pca9685.h"
|
|
|
|
#include <unistd.h>
|
|
|
|
#include "extern-plugininfo.h"
|
|
|
|
#include <QThread>
|
|
#include <qmath.h>
|
|
|
|
Pca9685::Pca9685(const QString &portName, int address, QObject *parent) : I2CDevice(portName, address, parent)
|
|
{
|
|
|
|
}
|
|
|
|
bool Pca9685::writeData(int fd, const QByteArray &command)
|
|
{
|
|
if (command == "init") {
|
|
writeByte8(fd, MODE2, OUTDRV);
|
|
writeByte8(fd, MODE1, ALLCALL);
|
|
QThread::msleep(5);
|
|
quint8 mode1 = readByte8(fd, MODE1);
|
|
mode1 = mode1 & ~SLEEP;
|
|
writeByte8(fd, MODE1, mode1);
|
|
QThread::msleep(5);
|
|
qCInfo(dcI2cDevices())<<"PCA9685 Initialization: SUCCESS";
|
|
return true;
|
|
}
|
|
|
|
if (command.startsWith("PWM")) {
|
|
QList<QByteArray> parts = command.split(':');
|
|
quint8 channel = parts.at(1).toUInt();
|
|
quint16 on = parts.at(2).toUInt();
|
|
quint16 off = parts.at(3).toUInt();
|
|
writeByte8(fd, LED0_ON_L+4*channel, on & 0xFF);
|
|
writeByte8(fd, LED0_ON_H+4*channel, on >> 8);
|
|
writeByte8(fd, LED0_OFF_L+4*channel, off & 0xFF);
|
|
writeByte8(fd, LED0_OFF_H+4*channel, off >> 8);
|
|
return true;
|
|
}
|
|
|
|
if (command.startsWith("FREQ")) {
|
|
QList<QByteArray> parts = command.split(':');
|
|
quint16 frequency = parts.at(1).toUInt();
|
|
float prescaleval = 25000000.0; //25MHz
|
|
prescaleval /= 4096.0; //12-bit
|
|
prescaleval /= float(frequency);
|
|
prescaleval -= 1.0;
|
|
qInfo()<<"Setting PWM frequency to "<<frequency<<" hz";
|
|
qInfo()<<"Estimated pre-scale: "<<prescaleval;
|
|
int prescale = (int)qFloor(prescaleval + 0.5);
|
|
qInfo()<<"Final pre-scale: "<<prescale;
|
|
uint8_t oldmode = readByte8(fd, MODE1);
|
|
uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep
|
|
writeByte8(fd, MODE1, newmode); //go to sleep
|
|
writeByte8(fd, PRESCALE, prescale);
|
|
writeByte8(fd, MODE1, oldmode);
|
|
//time.sleep(0.005) # wait
|
|
QThread::msleep(5);
|
|
writeByte8(fd, MODE1, oldmode | 0x80);
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool Pca9685::writeByte8(int fd, uint8_t registerAddress, uint8_t data)
|
|
{
|
|
uint8_t buffer[2] = {0};
|
|
buffer[0] = registerAddress & 0x00FF;
|
|
buffer[1] = data;
|
|
|
|
if (write(fd, buffer, 2) != 2){
|
|
qCDebug(dcI2cDevices())<<"I2C Transaction failed!";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
uint8_t Pca9685::readByte8(int fd, uint8_t registerAddress)
|
|
{
|
|
uint8_t buffer[1] = {0};
|
|
buffer[0] = registerAddress & 0x00FF;
|
|
write(fd, buffer, 1);
|
|
|
|
uint8_t result_buffer[1] = {0};
|
|
if (read(fd, result_buffer, 1) != 1){
|
|
qDebug()<<"I2C Transaction failed!";
|
|
return 0;
|
|
}
|
|
|
|
return result_buffer[0];
|
|
}
|