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

Add PWM output driver with raspberry pi+pca9685 module

sbg
crossa 8 years ago committed by Beat Küng
parent
6ee6165592
commit
c89351c453
5 changed files with 825 additions and 0 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. 43
      src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt
  2. 204
      src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp
  3. 73
      src/drivers/rpi_pca9685_pwm_out/PCA9685.h
  4. 382
      src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp
  5. 123
      src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h

43
src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt
Unescape View File

@ -0,0 +1,43 @@ @@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2015-2016 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__rpi_pca9685_pwm_out
MAIN rpi_pca9685_pwm_out
COMPILE_FLAGS
SRCS
PCA9685.cpp
rpi_pca9685_pwm_out.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

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

@ -0,0 +1,204 @@ @@ -0,0 +1,204 @@
/*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
* Name : PCA9685.cpp
* Original Author : Georgi Todorov
* Edited by : Tord Wessman
* Version :
* Created on : Dec 9, 2012
*
* Copyright © 2012 Georgi Todorov <terahz@geodar.com>
*/
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <unistd.h>
#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
*/
void PCA9685::init(int bus, int address)
{
_i2cbus = bus;
_i2caddr = address;
snprintf(busfile, sizeof(busfile), "/dev/i2c-%d", bus);
reset();
//usleep(10*1000);
}
PCA9685::PCA9685() :
_i2caddr(0x40),
_i2cbus(1),
busfile {},
dataBuffer {}
{
}
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);
}
}
//! 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) {
uint8_t prescale = (CLOCK_FREQ / 4096 / 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 newmode = (oldmode & 0x7F) | 0x10; //sleep
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);
}
}
//! PWM a single channel
/*!
\param led channel to set PWM value for
\param value 0-4095 value for PWM
*/
void PCA9685::setPWM(uint8_t led, int value)
{
setPWM(led, 0, value);
}
//! PWM a single channel with custom on time
/*!
\param led channel to set PWM value for
\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);
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);
}
}
//! 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;
if (write(fd, buff, BUFFER_SIZE) != BUFFER_SIZE) {
printf("I2C slave 0x%x failed to go to register 0x%x [read_byte():write %d]", _i2caddr, address, errno);
return (-1);
} else {
if (read(fd, dataBuffer, BUFFER_SIZE) != BUFFER_SIZE) {
printf("Could not read from I2C slave 0x%x, register 0x%x [read_byte():read %d]", _i2caddr, address, errno);
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) {
printf("Failed to write to I2C Slave 0x%x @ register 0x%x [write_byte():write %d]", _i2caddr, address, errno);
usleep(5000);
} else {
//printf("Wrote to I2C Slave 0x%x @ register 0x%x [0x%x]\n", _i2caddr, address, data);
}
}
//! 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);
return -1;
}
if (ioctl(fd, I2C_SLAVE, _i2caddr) < 0) {
printf("I2C slave %d failed [openfd():ioctl %d]", _i2caddr, errno);
return -1;
}
return fd;
}

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

@ -0,0 +1,73 @@ @@ -0,0 +1,73 @@
/*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
* Name : PCA9685.h
* Original Author : Georgi Todorov
* Edited by : Tord Wessman
* Version :
* Created on : Nov 22, 2013
*
* Copyright © 2012 Georgi Todorov <terahz@geodar.com>
*/
#ifndef _PCA9685_H
#define _PCA9685_H
#include <inttypes.h>
// Register Definitions
#define MODE1 0x00 //Mode register 1
#define MODE2 0x01 //Mode register 2
#define SUBADR1 0x02 //I2C-bus subaddress 1
#define SUBADR2 0x03 //I2C-bus subaddress 2
#define SUBADR3 0x04 //I2C-bus subaddress 3
#define ALLCALLADR 0x05 //LED All Call I2C-bus address
#define LED0 0x6 //LED0 start register
#define LED0_ON_L 0x6 //LED0 output and brightness control byte 0
#define LED0_ON_H 0x7 //LED0 output and brightness control byte 1
#define LED0_OFF_L 0x8 //LED0 output and brightness control byte 2
#define LED0_OFF_H 0x9 //LED0 output and brightness control byte 3
#define LED_MULTIPLYER 4 // For the other 15 channels
#define ALLLED_ON_L 0xFA //load all the LEDn_ON registers, byte 0 (turn 0-7 channels on)
#define ALLLED_ON_H 0xFB //load all the LEDn_ON registers, byte 1 (turn 8-15 channels on)
#define ALLLED_OFF_L 0xFC //load all the LEDn_OFF registers, byte 0 (turn 0-7 channels off)
#define ALLLED_OFF_H 0xFD //load all the LEDn_OFF registers, byte 1 (turn 8-15 channels off)
#define PRE_SCALE 0xFE //prescaler for output frequency
#define CLOCK_FREQ 25000000.0 //25MHz default osc clock
#define BUFFER_SIZE 0x08 //1 byte buffer
//! Main class that exports features for PCA9685 chip
class PCA9685
{
public:
PCA9685();
void init(int, int);
virtual ~PCA9685();
void reset(void);
void setPWMFreq(int);
void setPWM(uint8_t, int, int);
void setPWM(uint8_t, int);
private:
int _i2caddr;
int _i2cbus;
char busfile[64];
uint8_t dataBuffer[BUFFER_SIZE];
uint8_t read_byte(int, uint8_t);
void write_byte(int, uint8_t, uint8_t);
int openfd();
};
#endif

382
src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp
Unescape View File

@ -0,0 +1,382 @@ @@ -0,0 +1,382 @@
/****************************************************************************
*
* Copyright (c) 2015-2016 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "rpi_pca9685_pwm_out.h"
using namespace rpi_pca9685_pwm_out;
//--------------------------------------------------------------------------//
int rpi_pca9685_pwm_out::mixer_control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
return 0;
}
//----------------------------------------------------------------------------//
int rpi_pca9685_pwm_out::initialize_mixer(const char *mixer_filename)
{
char buf[4096];
unsigned buflen = sizeof(buf);
memset(buf, '\0', buflen);
_mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls);
// PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
if (load_mixer_file(mixer_filename, buf, buflen) == 0) {
if (_mixer_group->load_from_buf(buf, buflen) == 0) {
PX4_INFO("Loaded mixer from file %s", mixer_filename);
return 0;
} else {
PX4_ERR("Unable to parse from mixer config file %s", mixer_filename);
}
} else {
PX4_ERR("Unable to load config file %s", mixer_filename);
}
if (_mixer_group->count() <= 0) {
PX4_ERR("Mixer initialization failed");
return -1;
}
return 0;
}
//----------------------------------------------------------------------------//
int rpi_pca9685_pwm_out::pwm_initialize()
{
/**************初始化PCA9685开始*************/
pwm.init(1, 0x40);
usleep(1000 * 100);
pwm.setPWMFreq(61);
usleep(1000 * 1000);
/**************初始化PCA9685结束************/
return 0;
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::pwm_deinitialize()
{
rpi_pca9685_pwm_out::pwm.reset();
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::send_outputs_pwm(const uint16_t *pwm)
{
//向PCA9685发送数据
int i;
for (i = 0; i < NUM_PWM; ++i) {
rpi_pca9685_pwm_out::pwm.setPWM(i, *(pwm + i));
}
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::subscribe()
{
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
/* set up ORB topic names */
_controls_topics[0] = ORB_ID(actuator_controls_0);
_controls_topics[1] = ORB_ID(actuator_controls_1);
_controls_topics[2] = ORB_ID(actuator_controls_2);
_controls_topics[3] = ORB_ID(actuator_controls_3);
// Subscribe for orb topics
for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_required & (1 << i)) {
PX4_DEBUG("subscribe to actuator_controls_%d", i);
_controls_subs[i] = orb_subscribe(_controls_topics[i]);
} else {
_controls_subs[i] = -1;
}
if (_controls_subs[i] >= 0) {
_poll_fds[_poll_fds_num].fd = _controls_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
}
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::task_main(int argc, char *argv[])
{
_is_running = true;
/***************初始化PCA9685************/
rpi_pca9685_pwm_out::pwm_initialize();
// Set up mixer
if (initialize_mixer(_mixer_filename) < 0) {
PX4_ERR("无法初始化通道混合配置文件");
return;
}
_mixer_group->groups_required(_groups_required);
// subscribe and set up polling
subscribe();
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
pwm_limit_init(&_pwm_limit);
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(_poll_fds, _poll_fds_num, 10);
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) {
continue;
}
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(10000);
continue;
}
/* get controls for required topics */
unsigned poll_id = 0;
for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_controls_subs[i] >= 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]);
}
poll_id++;
}
}
if (_mixer_group != nullptr) {
_outputs.timestamp = hrt_absolute_time();
/* do mixing */
_outputs.noutputs = _mixer_group->mix(_outputs.output,
_outputs.NUM_ACTUATOR_OUTPUTS,
NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs;
i < _outputs.NUM_ACTUATOR_OUTPUTS;
i++) {
_outputs.output[i] = NAN;
}
const uint16_t reverse_mask = 0;
uint16_t disarmed_pwm[4];
uint16_t min_pwm[4];
uint16_t max_pwm[4];
for (unsigned int i = 0; i < NUM_PWM; i++) {
disarmed_pwm[i] = _pwm_disarmed;
min_pwm[i] = _pwm_min;
max_pwm[i] = _pwm_max;
}
uint16_t pwm[4];
// TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed,
false/*_armed.prearmed*/,
_outputs.noutputs,
reverse_mask,
disarmed_pwm,
min_pwm,
max_pwm,
_outputs.output,
pwm,
&_pwm_limit);
if (_armed.lockdown) {
send_outputs_pwm(disarmed_pwm);
} else {
send_outputs_pwm(pwm);
}
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
} else {
PX4_ERR("Could not mix output! Exiting...");
_task_should_exit = true;
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
pwm_deinitialize();
for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_controls_subs[i] >= 0) {
orb_unsubscribe(_controls_subs[i]);
}
}
orb_unsubscribe(_armed_sub);
_is_running = false;
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::task_main_trampoline(int argc, char *argv[])
{
task_main(argc, argv);
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::start()
{
ASSERT(_task_handle == -1);
_task_should_exit = false;
/* start the task */
_task_handle = px4_task_spawn_cmd("pwm_out_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
warn("task start failed");
return;
}
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::stop()
{
_task_should_exit = true;
while (_is_running) {
usleep(200000);
PX4_INFO(".");
}
_task_handle = -1;
}
//----------------------------------------------------------------------------//
void rpi_pca9685_pwm_out::usage()
{
PX4_INFO("usage: pwm_out start [-d pwmdevice] -m mixerfile");
PX4_INFO(" -d pwmdevice : sysfs device for pwm generation");
PX4_INFO(" (default /sys/class/pwm/pwmchip0)");
PX4_INFO(" -m mixerfile : path to mixerfile");
PX4_INFO(" (default ROMFS/px4fmu_common/mixers/quad_x.main.mix)");
PX4_INFO(" pwm_out stop");
PX4_INFO(" pwm_out status");
}
//----------------------------------------------------------------------------//
int rpi_pca9685_pwm_out_main(int argc, char **argv)
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
char *verb = nullptr;
if (argc >= 2) {
verb = argv[1];
} else {
return 1;
}
while ((ch = px4_getopt(argc, argv, "d:m:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'd':
strncpy(rpi_pca9685_pwm_out::_device, myoptarg, sizeof(rpi_pca9685_pwm_out::_device));
break;
case 'm':
strncpy(rpi_pca9685_pwm_out::_mixer_filename, myoptarg, sizeof(rpi_pca9685_pwm_out::_mixer_filename));
break;
}
}
// gets the parameters for the esc's pwm
param_get(param_find("PWM_DISARMED"), &rpi_pca9685_pwm_out::_pwm_disarmed);
param_get(param_find("PWM_MIN"), &rpi_pca9685_pwm_out::_pwm_min);
param_get(param_find("PWM_MAX"), &rpi_pca9685_pwm_out::_pwm_max);
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (rpi_pca9685_pwm_out::_is_running) {
PX4_WARN("pwm_out already running");
return 1;
}
rpi_pca9685_pwm_out::start();
}
else if (!strcmp(verb, "stop")) {
if (!rpi_pca9685_pwm_out::_is_running) {
PX4_WARN("pwm_out is not running");
return 1;
}
rpi_pca9685_pwm_out::stop();
}
else if (!strcmp(verb, "status")) {
PX4_WARN("pwm_out is %s", rpi_pca9685_pwm_out::_is_running ? "running" : "not running");
return 0;
} else {
rpi_pca9685_pwm_out::usage();
return 1;
}
return 0;
}

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

@ -0,0 +1,123 @@ @@ -0,0 +1,123 @@
/****************************************************************************
*
* Copyright (c) 2015-2016 zhangfan. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <stdint.h>
#include <px4_tasks.h>
#include <px4_getopt.h>
#include <px4_posix.h>
#include <errno.h>
#include <cmath> // NAN
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/mixer/mixer_load.h>
#include <systemlib/param/param.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include "PCA9685.h"
namespace rpi_pca9685_pwm_out
{
static px4_task_t _task_handle = -1;
volatile bool _task_should_exit = false;
static bool _is_running = false;
static const int NUM_PWM = 4;
static char _device[64] = "/sys/class/pwm/pwmchip0";
//static int _pwm_fd[NUM_PWM];
static const int FREQUENCY_PWM = 400;
static char _mixer_filename[64] = "ROMFS/px4fmu_common/mixers/quad_x.main.mix";
// subscriptions
int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
int _armed_sub;
// publications
orb_advert_t _outputs_pub = nullptr;
orb_advert_t _rc_pub = nullptr;
// topic structures
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_outputs_s _outputs;
actuator_armed_s _armed;
// polling
uint8_t _poll_fds_num = 0;
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
// control groups related
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
pwm_limit_t _pwm_limit;
// esc parameters
int32_t _pwm_disarmed;
int32_t _pwm_min;
int32_t _pwm_max;
//PCA9685 device
::PCA9685 pwm;
MixerGroup *_mixer_group = nullptr;
void usage();
void start();
void stop();
int pwm_initialize();
void pwm_deinitialize();
void send_outputs_pwm(const uint16_t *pwm);
void task_main_trampoline(int argc, char *argv[]);
void subscribe();
void task_main(int argc, char *argv[]);
/* mixer initialization */
int initialize_mixer(const char *mixer_filename);
int mixer_control_callback(uintptr_t handle, uint8_t control_group,
uint8_t control_index, float &input);
}
extern "C" __EXPORT int rpi_pca9685_pwm_out_main(int, char **);
Write Preview
Loading…
Cancel
Save