From d17a7b46e89e477156de62012f17cde289a4b7b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 6 Jun 2017 16:40:53 +0200 Subject: [PATCH] linux_pwm_out: add ocpoc_mmap mode --- src/drivers/linux_pwm_out/CMakeLists.txt | 1 + src/drivers/linux_pwm_out/linux_pwm_out.cpp | 13 ++- src/drivers/linux_pwm_out/ocpoc_mmap.cpp | 108 ++++++++++++++++++++ src/drivers/linux_pwm_out/ocpoc_mmap.h | 75 ++++++++++++++ 4 files changed, 193 insertions(+), 4 deletions(-) create mode 100644 src/drivers/linux_pwm_out/ocpoc_mmap.cpp create mode 100644 src/drivers/linux_pwm_out/ocpoc_mmap.h diff --git a/src/drivers/linux_pwm_out/CMakeLists.txt b/src/drivers/linux_pwm_out/CMakeLists.txt index a5ce8123b6..8bc8470b03 100644 --- a/src/drivers/linux_pwm_out/CMakeLists.txt +++ b/src/drivers/linux_pwm_out/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( PCA9685.cpp navio_sysfs.cpp linux_pwm_out.cpp + ocpoc_mmap.cpp DEPENDS platforms__common ) diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 153c8edce5..89a792a711 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -54,6 +54,7 @@ #include "common.h" #include "navio_sysfs.h" #include "PCA9685.h" +#include "ocpoc_mmap.h" namespace linux_pwm_out { @@ -63,7 +64,7 @@ static bool _is_running = false; static char _device[64] = "/sys/class/pwm/pwmchip0"; static char _protocol[64] = "navio"; -static int _max_mum_outputs = 8; ///< maximum number of outputs the driver should use +static int _max_num_outputs = 8; ///< maximum number of outputs the driver should use static char _mixer_filename[64] = "ROMFS/px4fmu_common/mixers/quad_x.main.mix"; // subscriptions @@ -206,9 +207,13 @@ void task_main(int argc, char *argv[]) PX4_INFO("Starting PWM output in PCA9685 mode"); pwm_out = new PCA9685(); + } else if (strcmp(_protocol, "ocpoc_mmap") == 0) { + PX4_INFO("Starting PWM output in ocpoc_mmap mode"); + pwm_out = new OcpocMmapPWMOut(_max_num_outputs); + } else { // navio PX4_INFO("Starting PWM output in Navio mode"); - pwm_out = new NavioSysfsPWMOut(_device, _max_mum_outputs); + pwm_out = new NavioSysfsPWMOut(_device, _max_num_outputs); } if (pwm_out->init() != 0) { @@ -380,7 +385,7 @@ void usage() 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(" -p protocol : driver output protocol (navio|pca9685)"); + PX4_INFO(" -p protocol : driver output protocol (navio|pca9685|ocpoc_mmap)"); PX4_INFO(" (default is navio)"); PX4_INFO(" -n num_outputs : maximum number of outputs the driver should use"); PX4_INFO(" (default is 8)"); @@ -433,7 +438,7 @@ int linux_pwm_out_main(int argc, char *argv[]) max_num = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; } - linux_pwm_out::_max_mum_outputs = max_num; + linux_pwm_out::_max_num_outputs = max_num; } break; } diff --git a/src/drivers/linux_pwm_out/ocpoc_mmap.cpp b/src/drivers/linux_pwm_out/ocpoc_mmap.cpp new file mode 100644 index 0000000000..c301f77be1 --- /dev/null +++ b/src/drivers/linux_pwm_out/ocpoc_mmap.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 + * 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 "ocpoc_mmap.h" + +#include + +#include +#include + +using namespace linux_pwm_out; + +#define RCOUT_ZYNQ_PWM_BASE 0x43c00000 +static const int TICK_PER_US = 50; +static const int FREQUENCY_PWM = 400; +static const int TICK_PER_S = 50000000; + +OcpocMmapPWMOut::OcpocMmapPWMOut(int max_num_outputs) +{ + _num_outputs = max_num_outputs; + + if (_num_outputs > MAX_ZYNQ_PWMS) { + PX4_WARN("number of outputs too large. Setting to %i", MAX_ZYNQ_PWMS); + _num_outputs = MAX_ZYNQ_PWMS; + } +} + +OcpocMmapPWMOut::~OcpocMmapPWMOut() +{ + if (_shared_mem_cmd) { + munmap((void *)_shared_mem_cmd, 0x1000); + } +} + +unsigned long OcpocMmapPWMOut::freq2tick(uint16_t freq_hz) +{ + unsigned long duty = TICK_PER_S / (unsigned long)freq_hz; + return duty; +} + +int OcpocMmapPWMOut::init() +{ + uint32_t mem_fd = open(_device, O_RDWR | O_SYNC); + _shared_mem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ | PROT_WRITE, + MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE); + close(mem_fd); + + if (_shared_mem_cmd == nullptr) { + PX4_ERR("initialize pwm pointer failed."); + return -1; + } + + for (int i = 0; i < _num_outputs; ++i) { + _shared_mem_cmd->periodhi[i].period = freq2tick(FREQUENCY_PWM); + _shared_mem_cmd->periodhi[i].hi = freq2tick(FREQUENCY_PWM) / 2; + PX4_DEBUG("Output values: %d, %d", _shared_mem_cmd->periodhi[i].period, _shared_mem_cmd->periodhi[i].hi); + } + + return 0; +} + +int OcpocMmapPWMOut::send_output_pwm(const uint16_t *pwm, int num_outputs) +{ + if (num_outputs > _num_outputs) { + num_outputs = _num_outputs; + } + + //convert this to duty_cycle in ns + for (int i = 0; i < num_outputs; ++i) { + //n = ::asprintf(&data, "%u", pwm[i] * 1000); + //::write(_pwm_fd[i], data, n); + _shared_mem_cmd->periodhi[i].hi = TICK_PER_US * pwm[i]; + //printf("ch:%d, val:%d*%d ", ch, period_us, TICK_PER_US); + } + + return 0; +} + diff --git a/src/drivers/linux_pwm_out/ocpoc_mmap.h b/src/drivers/linux_pwm_out/ocpoc_mmap.h new file mode 100644 index 0000000000..aa669ce07d --- /dev/null +++ b/src/drivers/linux_pwm_out/ocpoc_mmap.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "common.h" + +namespace linux_pwm_out +{ + +/** + ** class OcpocMmapPWMOut + * PWM output class for Aerotenna OcPoC via mmap + */ +class OcpocMmapPWMOut : public PWMOutBase +{ +public: + OcpocMmapPWMOut(int max_num_outputs); + virtual ~OcpocMmapPWMOut(); + + int init() override; + + int send_output_pwm(const uint16_t *pwm, int num_outputs) override; + +private: + static unsigned long freq2tick(uint16_t freq_hz); + + static constexpr int MAX_ZYNQ_PWMS = 8; /**< maximum number of pwm channels */ + + // Period|Hi 32 bits each + struct s_period_hi { + uint32_t period; + uint32_t hi; + }; + + struct pwm_cmd { + struct s_period_hi periodhi[MAX_ZYNQ_PWMS]; + }; + + volatile struct pwm_cmd *_shared_mem_cmd = nullptr; + static constexpr const char *_device = "/dev/mem"; + int _num_outputs; +}; + +}