diff --git a/src/drivers/boards/ocpoc/board_config.h b/src/drivers/boards/ocpoc/board_config.h new file mode 100644 index 0000000000..c22b6191c7 --- /dev/null +++ b/src/drivers/boards/ocpoc/board_config.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * OCPOC internal definitions + */ + +#pragma once + +#define BOARD_OVERRIDE_UUID "OCPOC " // must be of length 12 (PX4_CPU_UUID_BYTE_LENGTH) +#define BOARD_OVERRIDE_MFGUID BOARD_OVERRIDE_UUID + +#define BOARD_NAME "OCPOC" +#define BOARD_BATTERY1_V_DIV (10.177939394f) +#define BOARD_HAS_NO_RESET +#define BOARD_HAS_NO_BOOTLOADER + +// Battery ADC channels +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 + +#include +#include "../common/board_common.h" + +#define BOARD_MAX_LEDS 1 // Number external of LED's this board has diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 5d110d96ff..bf51e10064 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -57,5 +57,8 @@ // set the queue size to the number of LED's, so that each led can be controlled individually static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS; - +#if defined (__PX4_POSIX_OCPOC) +#define RGBLED0_DEVICE_PATH "/dev/i2c-1" +#else #define RGBLED0_DEVICE_PATH "/dev/rgbled0" +#endif diff --git a/src/drivers/ocpoc_adc/CMakeLists.txt b/src/drivers/ocpoc_adc/CMakeLists.txt new file mode 100644 index 0000000000..6beac74669 --- /dev/null +++ b/src/drivers/ocpoc_adc/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ +px4_add_module( + MODULE drivers__ocpoc_adc + MAIN ocpoc_adc + COMPILE_FLAGS + SRCS + ocpoc_adc.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ocpoc_adc/ocpoc_adc.cpp b/src/drivers/ocpoc_adc/ocpoc_adc.cpp new file mode 100644 index 0000000000..2a4a8bf2f0 --- /dev/null +++ b/src/drivers/ocpoc_adc/ocpoc_adc.cpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file ocpoc_adc.cpp + * + * OcPoC ADC Driver + * + * @author Lianying Ji + * @author Dave Royer + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#define ADC_BASE_DEV_PATH "/dev/adc" +#define ADC_VOLTAGE_PATH "/sys/bus/iio/devices/iio:device0/in_voltage8_raw" + +__BEGIN_DECLS +__EXPORT int ocpoc_adc_main(int argc, char *argv[]); +__END_DECLS + +class OcpocADC: public DriverFramework::VirtDevObj +{ +public: + OcpocADC(); + virtual ~OcpocADC(); + + virtual int init(); + + virtual ssize_t devRead(void *buf, size_t count) override; + virtual int devIOCTL(unsigned long request, unsigned long arg) override; + +protected: + virtual void _measure() override; + +private: + int read(struct adc_msg_s(*buf)[12], unsigned int len); + + pthread_mutex_t _samples_lock; + adc_msg_s _samples; + FILE *xadc_fd; +}; + +OcpocADC::OcpocADC() + : DriverFramework::VirtDevObj("ocpoc_adc", ADC0_DEVICE_PATH, ADC_BASE_DEV_PATH, 1e6 / 100) +{ + pthread_mutex_init(&_samples_lock, NULL); +} + +OcpocADC::~OcpocADC() +{ + pthread_mutex_destroy(&_samples_lock); +} + +void OcpocADC::_measure() +{ + struct adc_msg_s tmp_samples[12]; + + int ret = read(&tmp_samples, sizeof(tmp_samples)); + + if (ret != 0) { + PX4_ERR("ocpoc_adc_read: %d", ret); + } + + pthread_mutex_lock(&_samples_lock); + memcpy(&_samples, &tmp_samples, sizeof(tmp_samples)); + pthread_mutex_unlock(&_samples_lock); +} + +int OcpocADC::init() +{ + int ret; + + ret = DriverFramework::VirtDevObj::init(); + + if (ret != PX4_OK) { + PX4_ERR("init failed"); + return ret; + } + + return PX4_OK; +} + +int OcpocADC::devIOCTL(unsigned long request, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t OcpocADC::devRead(void *buf, size_t count) +{ + const size_t maxsize = sizeof(_samples); + int ret; + + if (count > maxsize) { + count = maxsize; + } + + ret = pthread_mutex_trylock(&_samples_lock); + + if (ret != 0) { + return 0; + } + + memcpy(buf, &_samples, count); + pthread_mutex_unlock(&_samples_lock); + + return count; +} + +int OcpocADC::read(struct adc_msg_s(*buf)[12], unsigned int len) +{ + uint32_t buff[1]; + int ret = 0; + + xadc_fd = fopen(ADC_VOLTAGE_PATH, "r"); + + if (xadc_fd != NULL) { + fscanf(xadc_fd, "%d", buff); + fclose(xadc_fd); + + (*buf)[0].am_data = buff[0]; + + } else { + (*buf)[0].am_data = 0; + ret = -1; + } + + (*buf)[0].am_channel = ADC_BATTERY_VOLTAGE_CHANNEL; + + return ret; +} + +static OcpocADC *instance = nullptr; + +int ocpoc_adc_main(int argc, char *argv[]) +{ + int ret; + + if (argc < 2) { + PX4_WARN("usage: {start|stop|test}"); + return PX4_ERROR; + } + + if (!strcmp(argv[1], "start")) { + if (instance) { + PX4_WARN("already started"); + return PX4_OK; + } + + instance = new OcpocADC; + + if (!instance) { + PX4_WARN("not enough memory"); + return PX4_ERROR; + } + + if (instance->init() != PX4_OK) { + delete instance; + instance = nullptr; + PX4_WARN("init failed"); + return PX4_ERROR; + } + + return PX4_OK; + + } else if (!strcmp(argv[1], "stop")) { + if (!instance) { + PX4_WARN("already stopped"); + return PX4_OK; + } + + delete instance; + instance = nullptr; + return PX4_OK; + + } else if (!strcmp(argv[1], "test")) { + if (!instance) { + PX4_ERR("start first"); + return PX4_ERROR; + } + + struct adc_msg_s adc_msgs[12]; + + ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs)); + + if (ret < 0) { + PX4_ERR("ret: %s (%d)\n", strerror(ret), ret); + return ret; + + } else { + PX4_INFO("ADC Data: %d", adc_msgs[0].am_data); + } + + return PX4_OK; + + } else { + PX4_WARN("action (%s) not supported", argv[1]); + + return PX4_ERROR; + } + + return PX4_OK; + +} diff --git a/src/drivers/ocpoc_mmap_pwm_out/CMakeLists.txt b/src/drivers/ocpoc_mmap_pwm_out/CMakeLists.txt new file mode 100644 index 0000000000..3f17ff6a81 --- /dev/null +++ b/src/drivers/ocpoc_mmap_pwm_out/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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__ocpoc_mmap_pwm_out + MAIN ocpoc_mmap_pwm_out + COMPILE_FLAGS -Os + SRCS + ocpoc_mmap_pwm_out.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp b/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp new file mode 100644 index 0000000000..71e9c59401 --- /dev/null +++ b/src/drivers/ocpoc_mmap_pwm_out/ocpoc_mmap_pwm_out.cpp @@ -0,0 +1,527 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + + +namespace ocpoc_mmap_pwm_out +{ +#define RCOUT_ZYNQ_PWM_BASE 0x43c00000 +#define MAX_ZYNQ_PWMS 8 /* number of pwm channels */ +static const int TICK_PER_US = 50; +static const int NUM_PWM = 8; + + +static px4_task_t _task_handle = -1; +volatile bool _task_should_exit = false; +static bool _is_running = false; + +// 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 *sharedMem_cmd = nullptr; // jly +static char _device[32] = "/dev/mem"; + +static const int TICK_PER_S = 50000000; + +static const int FREQUENCY_PWM = 400; +static const char *MIXER_FILENAME = "/home/root/ROMFS/px4fmu_common/mixers/ocpoc_quad_x.main.mix"; + +// subscriptions +int _controls_sub; +int _armed_sub; +int _rc_channels_sub; // 2016-10-28 for esc calib + +// publications +orb_advert_t _outputs_pub = nullptr; +orb_advert_t _rc_pub = nullptr; + +// topic structures +actuator_controls_s _controls; +actuator_outputs_s _outputs; +actuator_armed_s _armed; + +pwm_limit_t _pwm_limit; + +// esc parameters +int32_t _pwm_disarmed; +int32_t _pwm_min; +int32_t _pwm_max; + +MultirotorMixer *_mixer = nullptr; + +void usage(); + +void start(); + +void stop(); + +int pwm_write_sysfs(char *path, int value); + +unsigned long freq2tick(uint16_t freq_hz); + +int pwm_initialize(const char *device); + +void pwm_deinitialize(); + +void send_outputs_pwm(const uint16_t *pwm); + +void task_main_trampoline(int argc, char *argv[]); + +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); + + +int 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 initialize_mixer(const char *mixer_filename) +{ + char buf[2048]; + unsigned buflen = sizeof(buf); + + PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); + + int fd_load = ::open(mixer_filename, O_RDONLY); + + if (fd_load != -1) { + int nRead = ::read(fd_load, buf, buflen); + close(fd_load); + + if (nRead > 0) { + _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); + + if (_mixer != nullptr) { + PX4_INFO("Successfully initialized mixer from config file"); + return 0; + + } else { + PX4_ERR("Unable to parse from mixer config file"); + return -1; + } + + } else { + PX4_WARN("Unable to read from mixer config file"); + return -2; + } + + } else { + PX4_WARN("No mixer config file found, using default mixer."); + + /* Mixer file loading failed, fall back to default mixer configuration for + * QUAD_X airframe. */ + float roll_scale = 1; + float pitch_scale = 1; + float yaw_scale = 1; + float deadband = 0.13; + + _mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, + MultirotorGeometry::QUAD_X, + roll_scale, pitch_scale, yaw_scale, deadband); + + // TODO: temporary hack to make this compile + (void)_config_index[0]; + + if (_mixer == nullptr) { + PX4_ERR("Mixer initialization failed"); + return -1; + } + + return 0; + } +} + +unsigned long freq2tick(uint16_t freq_hz) +{ + unsigned long duty = TICK_PER_S/(unsigned long)freq_hz; + return duty; +} + + +int pwm_initialize(const char *device) +{ + + int i; + uint32_t mem_fd; + //signal(SIGBUS,catch_sigbus); + mem_fd = open(device, O_RDWR|O_SYNC); + sharedMem_cmd = (struct pwm_cmd *) mmap(0, 0x1000, PROT_READ|PROT_WRITE, + MAP_SHARED, mem_fd, RCOUT_ZYNQ_PWM_BASE); + close(mem_fd); + + if (sharedMem_cmd == nullptr) { + PX4_ERR("initialize pwm pointer failed."); + return -1; + } + + for (i = 0; i < NUM_PWM; ++i) { + sharedMem_cmd->periodhi[i].period = freq2tick(FREQUENCY_PWM); + sharedMem_cmd->periodhi[i].hi = freq2tick(FREQUENCY_PWM)/2; // i prefer it is zero at the beginning + //PX4_ERR("initialize pwm pointer failed.%d, %d", sharedMem_cmd->periodhi[i].period, sharedMem_cmd->periodhi[i].hi); + } + + return 0; +} + +void pwm_deinitialize() +{ + for (int i = 0; i < NUM_PWM; ++i) { + sharedMem_cmd = nullptr; + } +} + + +void send_outputs_pwm(const uint16_t *pwm) +{ + if (sharedMem_cmd == nullptr) { + PX4_ERR("write pwm when pwm pointer is not initialized."); + return; + } + + //convert this to duty_cycle in ns + for (unsigned i = 0; i < NUM_PWM; ++i) { + //n = ::asprintf(&data, "%u", pwm[i] * 1000); + //::write(_pwm_fd[i], data, n); + sharedMem_cmd->periodhi[i].hi = TICK_PER_US*pwm[i]; + //printf("ch:%d, val:%d*%d ", ch, period_us, TICK_PER_US); + } +} + + + +void task_main(int argc, char *argv[]) +{ + _is_running = true; + struct rc_channels_s _rc; /**< r/c channel data */ + + if (pwm_initialize(_device) < 0) { + PX4_ERR("Failed to initialize PWM."); + return; + } + + // Subscribe for orb topics + _controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + _rc_channels_sub = orb_subscribe(ORB_ID(rc_channels)); + + // Start disarmed + _armed.armed = false; + _armed.prearmed = false; + + px4_pollfd_struct_t fds[1]; + fds[0].events = POLLIN; + + /* Don't limit poll intervall for now, 250 Hz should be fine. */ + //orb_set_interval(_controls_sub, 10); + + // Set up mixer + if (initialize_mixer(MIXER_FILENAME) < 0) { + PX4_ERR("Mixer initialization failed."); + return; + } + + pwm_limit_init(&_pwm_limit); + + // Main loop + while (!_task_should_exit) { + // Set up poll topic + if (_armed.in_esc_calibration_mode == true) { + fds[0].fd = _rc_channels_sub; + }else{ + fds[0].fd = _controls_sub; + } + + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 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(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + if (_armed.in_esc_calibration_mode == true) { + orb_copy(ORB_ID(rc_channels), _rc_channels_sub, &_rc); + _controls.control[0] = 0; + _controls.control[1] = 0; + _controls.control[2] = 0; + _controls.control[3] = _rc.channels[_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE]]; + }else{ + orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); + } + + _outputs.timestamp = _controls.timestamp; + + /* do mixing */ + _outputs.noutputs = _mixer->mix(_outputs.output, + 0 /* not used */, + NULL); + + + /* disable unused ports by setting their output to NaN */ + for (size_t i = _outputs.noutputs; + i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); + 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 < 4; 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 if (_armed.in_esc_calibration_mode) { + if (_controls.control[3]*1000 > 0.5f) { + pwm[0] = _pwm_max; + pwm[1] = _pwm_max; + pwm[2] = _pwm_max; + pwm[3] = _pwm_max; + }else{ + pwm[0] = _pwm_min; + pwm[1] = _pwm_min; + pwm[2] = _pwm_min; + pwm[3] = _pwm_min; + } + send_outputs_pwm(pwm); + PX4_WARN("calib pwm %d:%d:%d:%d.", pwm[0],pwm[1],pwm[2],pwm[3]); + } 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); + } + } + + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } + } + + pwm_deinitialize(); + orb_unsubscribe(_controls_sub); + orb_unsubscribe(_armed_sub); + + _is_running = false; + +} + +void task_main_trampoline(int argc, char *argv[]) +{ + task_main(argc, argv); +} + +void 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 stop() +{ + _task_should_exit = true; + + while (_is_running) { + usleep(200000); + PX4_INFO("."); + } + + _task_handle = -1; +} + +void usage() +{ + PX4_INFO("usage: pwm_out start -d /sys/class/pwm/pwmchip0"); + PX4_INFO(" pwm_out stop"); + PX4_INFO(" pwm_out status"); +} + +} // namespace navio_sysfs_pwm_out + +/* driver 'main' command */ +extern "C" __EXPORT int ocpoc_mmap_pwm_out_main(int argc, char *argv[]); + +int ocpoc_mmap_pwm_out_main(int argc, char *argv[]) +{ + const char *device = nullptr; + 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:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device = myoptarg; + strncpy(ocpoc_mmap_pwm_out::_device, device, strlen(device)); + break; + } + } + + // gets the parameters for the esc's pwm + param_get(param_find("PWM_DISARMED"), &ocpoc_mmap_pwm_out::_pwm_disarmed); + param_get(param_find("PWM_MIN"), &ocpoc_mmap_pwm_out::_pwm_min); + param_get(param_find("PWM_MAX"), &ocpoc_mmap_pwm_out::_pwm_max); + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + if (ocpoc_mmap_pwm_out::_is_running) { + PX4_WARN("pwm_out already running"); + return 1; + } + + ocpoc_mmap_pwm_out::start(); + } + + else if (!strcmp(verb, "stop")) { + if (!ocpoc_mmap_pwm_out::_is_running) { + PX4_WARN("pwm_out is not running"); + return 1; + } + + ocpoc_mmap_pwm_out::stop(); + } + + else if (!strcmp(verb, "status")) { + PX4_WARN("pwm_out is %s", ocpoc_mmap_pwm_out::_is_running ? "running" : "not running"); + return 0; + + } else { + ocpoc_mmap_pwm_out::usage(); + return 1; + } + + return 0; +} diff --git a/src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt b/src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt new file mode 100644 index 0000000000..e58f88f4b2 --- /dev/null +++ b/src/drivers/ocpoc_sbus_rc_in/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 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__ocpoc_sbus_rc_in + MAIN ocpoc_sbus_rc_in + STACK_MAIN 1200 + COMPILE_FLAGS -Os + SRCS + ocpoc_sbus_rc_in.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp b/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp new file mode 100644 index 0000000000..1372fd14ba --- /dev/null +++ b/src/drivers/ocpoc_sbus_rc_in/ocpoc_sbus_rc_in.cpp @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 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 +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include + +namespace ocpoc_sbus_rc_in +{ + +extern "C" __EXPORT int ocpoc_sbus_rc_in_main(int argc, char *argv[]); + +#define RCINPUT_DEVICE_PATH "/dev/ttyS2" + +#define RCINPUT_MEASURE_INTERVAL_US 6500 // FUBATA T8J is 6.8ms frame rate, microseconds + +#define SBUS_INPUT_CHANNELS 8 // FUBATA T8J is 8-channel + +class RcInput +{ +public: + RcInput() : + _shouldExit(false), + _isRunning(false), + _work{}, + _rcinput_pub(nullptr), + _channels(8), //FUBUTA, 8 + _data{} + { + _sbus_fd = -1; + } + ~RcInput() + { + work_cancel(HPWORK, &_work); + _isRunning = false; + } + + /* @return 0 on success, -errno on failure */ + int start(); + + /* @return 0 on success, -errno on failure */ + void stop(); + + /* Trampoline for the work queue. */ + static void cycle_trampoline(void *arg); + + bool isRunning() { return _isRunning; } + +private: + void _cycle(); + void _measure(); + + bool _shouldExit; + bool _isRunning; + struct work_s _work; + + orb_advert_t _rcinput_pub; + + int _channels; + int _sbus_fd = -1; + struct input_rc_s _data; + + int ocpoc_subs_rc_init(); +}; + +int RcInput::ocpoc_subs_rc_init() +{ + int i; + + /* S.bus input (USART3) */ + _sbus_fd = sbus_init(RCINPUT_DEVICE_PATH, true); //jly + + + for (i=_channels; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { + _data.values[i] = UINT16_MAX; + } + + _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); + + if (_rcinput_pub == nullptr) { + PX4_WARN("error: advertise failed"); + return -1; + } + + return 0; +} + +int RcInput::start() +{ + int result = 0; + + result = ocpoc_subs_rc_init(); + + if (result != 0) { + PX4_WARN("error: RC sbus initialization failed"); + return -1; + } + + _isRunning = true; + result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0); + + if (result == -1) { + _isRunning = false; + } + + return result; +} + +void RcInput::stop() +{ + _shouldExit = true; +} + +void RcInput::cycle_trampoline(void *arg) +{ + RcInput *dev = reinterpret_cast(arg); + dev->_cycle(); +} + +void RcInput::_cycle() +{ + _measure(); + + if (!_shouldExit) { + work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, + USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); + } +} + +void RcInput::_measure(void) +{ + uint64_t ts; + + /* + * Gather R/C control inputs from sbus + */ + bool sbus_failsafe, sbus_frame_drop; + uint16_t values[SBUS_INPUT_CHANNELS*2]; + uint16_t num_values; + + bool sbus_updated = sbus_input(_sbus_fd, values, &num_values, &sbus_failsafe, &sbus_frame_drop, + _channels); + + if (sbus_updated) { + + if (num_values > 8) { + num_values = 8; + } + + // assume r_raw_rc_count may be less than _channels + for (int i = 0; i < num_values; ++i) { + _data.values[i] = values[i]; + } + + ts = hrt_absolute_time(); + _data.timestamp = ts; + _data.timestamp_last_signal = ts; + _data.channel_count = num_values; + _data.rssi = 100; + _data.rc_lost_frame_count = 0; + _data.rc_total_frame_count = 1; + _data.rc_ppm_frame_length = 100; + _data.rc_failsafe = sbus_failsafe; + _data.rc_lost = sbus_frame_drop; + _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; + + orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); + + } +} + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PX4_INFO("usage: ocpoc_sbus_rc_in {start|stop|status}"); +} + +static RcInput *rc_input = nullptr; + +int ocpoc_sbus_rc_in_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage("missing command"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (rc_input != nullptr && rc_input->isRunning()) { + PX4_WARN("already running"); + /* this is not an error */ + return 0; + } + + rc_input = new RcInput(); + + // Check if alloc worked. + if (rc_input == nullptr) { + PX4_ERR("alloc failed"); + return -1; + } + + int ret = rc_input->start(); + + if (ret != 0) { + PX4_ERR("start failed"); + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + + if (rc_input == nullptr || !rc_input->isRunning()) { + PX4_WARN("not running"); + /* this is not an error */ + return 0; + } + + rc_input->stop(); + + // Wait for task to die + int i = 0; + + do { + /* wait up to 3s */ + usleep(100000); + + } while (rc_input->isRunning() && ++i < 30); + + delete rc_input; + rc_input = nullptr; + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (rc_input != nullptr && rc_input->isRunning()) { + PX4_INFO("running"); + + } else { + PX4_INFO("not running\n"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; + +} + +}; // namespace navio_sysfs_rc_in