mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 04:03:35 +08:00
drivers: add support for Aerotenna OcPoC-Zynq hardware
This commit is contained in:
davidaroyer
committed by
Lorenz Meier
Lorenz Meier
parent
dbbe3c0863
commit
0510cd5992
@@ -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 <system_config.h>
|
||||
#include "../common/board_common.h"
|
||||
|
||||
#define BOARD_MAX_LEDS 1 // Number external of LED's this board has
|
||||
@@ -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
|
||||
|
||||
@@ -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 :
|
||||
@@ -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 <ji@aerotenna.com>
|
||||
* @author Dave Royer <dave@aerotenna.com>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_tasks.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_adc.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
|
||||
#include <VirtDevObj.hpp>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <string.h>
|
||||
|
||||
#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;
|
||||
|
||||
}
|
||||
@@ -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 :
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
#include <px4_tasks.h>
|
||||
#include <px4_getopt.h>
|
||||
#include <px4_posix.h>
|
||||
#include <errno.h>
|
||||
|
||||
#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_multirotor.generated.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/pwm_limit/pwm_limit.h>
|
||||
|
||||
#include <sys/mman.h>
|
||||
#include <signal.h>
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -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 :
|
||||
@@ -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 <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
|
||||
#include <rc/sbus.h>
|
||||
|
||||
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<RcInput *>(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
|
||||
Reference in New Issue
Block a user