yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fmu interface to Capture

Browse Source
This commit is contained in:
David Sidrane
2016-01-25 18:12:16 -10:00
committed by Lorenz Meier
parent b3af469e80
commit 8268161b18
1 changed files with 305 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

309
src/drivers/px4fmu/fmu.cpp
View File

@@ -57,6 +57,7 @@
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
@@ -70,6 +71,7 @@
#include <systemlib/param/param.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_input_capture.h>
#include <lib/rc/sbus.h>
#include <lib/rc/dsm.h>
@@ -99,9 +101,15 @@ public:
enum Mode {
MODE_NONE,
MODE_2PWM,
MODE_2PWM2CAP,
MODE_3PWM,
MODE_3PWM1CAP,
MODE_4PWM,
MODE_6PWM,
MODE_8PWM,
MODE_4CAP,
MODE_5CAP,
MODE_6CAP,
};
PX4FMU();
virtual ~PX4FMU();
@@ -118,6 +126,10 @@ public:
int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
static void capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow);
private:
enum RC_SCAN {
RC_SCAN_PPM = 0,
@@ -185,7 +197,6 @@ private:
static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
static void cycle_trampoline(void *arg);
void cycle();
void work_start();
void work_stop();
@@ -194,6 +205,8 @@ private:
uint8_t control_group,
uint8_t control_index,
float &input);
void capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
void subscribe();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@@ -217,6 +230,8 @@ private:
uint32_t gpio_read(void);
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int capture_ioctl(file *filp, int cmd, unsigned long arg);
/* do not allow to copy due to ptr data members */
PX4FMU(const PX4FMU &);
PX4FMU operator=(const PX4FMU &);
@@ -411,6 +426,11 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM
up_input_capture_set(2, Rising, 0, NULL, NULL);
up_input_capture_set(3, Rising, 0, NULL, NULL);
DEVICE_DEBUG("MODE_2PWM2CAP");
// no break
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_2PWM");
@@ -425,6 +445,23 @@ PX4FMU::set_mode(Mode mode)
break;
case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_3PWM1CAP");
up_input_capture_set(3, Rising, 0, NULL, NULL);
// no break
case MODE_3PWM: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_3PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
/* XXX magic numbers */
up_pwm_servo_init(0x7);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
DEVICE_DEBUG("MODE_4PWM");
@@ -641,6 +678,19 @@ PX4FMU::cycle_trampoline(void *arg)
dev->cycle();
}
void
PX4FMU::capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
PX4FMU *dev = reinterpret_cast<PX4FMU *>(context);
dev->capture_callback(chan_index, edge_time, edge_state, overflow);
}
void PX4FMU::capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state,overflow);
}
void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,
@@ -812,9 +862,15 @@ PX4FMU::cycle()
switch (_mode) {
case MODE_2PWM:
case MODE_2PWM2CAP:
num_outputs = 2;
break;
case MODE_3PWM:
case MODE_3PWM1CAP:
num_outputs = 3;
break;
case MODE_4PWM:
num_outputs = 4;
break;
@@ -1208,10 +1264,20 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
/* try it as a Capture ioctl next */
ret = capture_ioctl(filp, cmd, arg);
if (ret != -ENOTTY) {
return ret;
}
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch (_mode) {
case MODE_2PWM:
case MODE_3PWM:
case MODE_4PWM:
case MODE_2PWM2CAP:
case MODE_3PWM1CAP:
case MODE_6PWM:
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
@@ -1469,12 +1535,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_SET(3):
case PWM_SERVO_SET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(2):
if (_mode < MODE_3PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
@@ -1507,11 +1579,16 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(3):
case PWM_SERVO_GET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(2):
if (_mode < MODE_3PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(1):
@@ -1550,7 +1627,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(unsigned *)arg = 4;
break;
case MODE_3PWM:
case MODE_3PWM1CAP:
*(unsigned *)arg = 3;
break;
case MODE_2PWM:
case MODE_2PWM2CAP:
*(unsigned *)arg = 2;
break;
@@ -1578,6 +1661,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_2PWM);
break;
case 3:
set_mode(MODE_3PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
@@ -2052,6 +2139,124 @@ PX4FMU::gpio_read(void)
return bits;
}
int
PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -EINVAL;
lock();
input_capture_config_t *pconfig = 0;
input_capture_stats_t *stats = (input_capture_stats_t *)arg;
if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP)
{
pconfig = (input_capture_config_t *)arg;
}
switch (cmd) {
case INPUT_CAP_SET:
if (pconfig)
{
ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter,
pconfig->callback, pconfig->context);
}
break;
case INPUT_CAP_SET_CALLBACK:
if (pconfig)
{
ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context);
}
break;
case INPUT_CAP_GET_CALLBACK:
if (pconfig)
{
ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context);
}
break;
case INPUT_CAP_GET_STATS:
if (arg)
{
ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false);
}
break;
case INPUT_CAP_GET_CLR_STATS:
if (arg)
{
ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true);
}
break;
case INPUT_CAP_SET_EDGE:
if (pconfig)
{
ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge);
}
break;
case INPUT_CAP_GET_EDGE:
if (pconfig)
{
ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge);
}
break;
case INPUT_CAP_SET_FILTER:
if (pconfig)
{
ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter);
}
break;
case INPUT_CAP_GET_FILTER:
if (pconfig)
{
ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter);
}
break;
case INPUT_CAP_GET_COUNT:
ret = OK;
switch(_mode) {
case MODE_3PWM1CAP:
*(unsigned *)arg = 1;
break;
case MODE_2PWM2CAP:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
}
break;
case INPUT_CAP_SET_COUNT:
ret = OK;
switch(_mode) {
case MODE_3PWM1CAP:
set_mode(MODE_3PWM1CAP);
break;
case MODE_2PWM2CAP:
set_mode(MODE_2PWM2CAP);
break;
default:
ret = -EINVAL;
break;
}
break;
default:
ret = -ENOTTY;
break;
}
unlock();
return ret;
}
int
PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
{
@@ -2135,6 +2340,11 @@ enum PortMode {
PORT_PWM_AND_SERIAL,
PORT_PWM_AND_GPIO,
PORT_PWM4,
PORT_PWM3,
PORT_PWM2,
PORT_PWM3CAP1,
PORT_PWM2CAP2,
PORT_CAPTURE,
};
PortMode g_port_mode;
@@ -2176,6 +2386,26 @@ fmu_new_mode(PortMode new_mode)
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
break;
case PORT_PWM3:
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_3PWM;
break;
case PORT_PWM3CAP1:
/* select 3-pin PWM mode 1 capture */
servo_mode = PX4FMU::MODE_3PWM1CAP;
break;
case PORT_PWM2:
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_2PWM;
break;
case PORT_PWM2CAP2:
/* select 2-pin PWM mode 2 capture */
servo_mode = PX4FMU::MODE_2PWM2CAP;
break;
#endif
/* mixed modes supported on v1 board only */
@@ -2304,9 +2534,15 @@ test(void)
{
int fd;
unsigned servo_count = 0;
unsigned capture_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
uint32_t rate_limit = 0;
struct input_capture_t {
bool valid;
input_capture_config_t chan;
} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
@@ -2320,7 +2556,32 @@ test(void)
err(1, "Unable to get servo count\n");
}
warnx("Testing %u servos", (unsigned)servo_count);
if (ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
fprintf(stdout,"Not in a capture mode\n");
}
warnx("Testing %u servos and %u input captures", (unsigned)servo_count, capture_count);
memset(capture_conf, 0, sizeof(capture_conf));
if (capture_count !=0 ) {
for(unsigned i = 0; i < capture_count; i++) {
// Map to channel number
capture_conf[i].chan.channel = i+ servo_count;
/* Save handler */
if (ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) {
err(1, "Unable to get capture callback for chan %u\n", capture_conf[i].chan.channel);
} else {
input_capture_config_t conf = capture_conf[i].chan;
conf.callback = &PX4FMU::capture_trampoline;
conf.context = g_fmu;
if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
capture_conf[i].valid = true;
} else {
err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
}
}
}
}
struct pollfd fds;
fds.fd = 0; /* stdin */
@@ -2382,7 +2643,26 @@ test(void)
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
}
if (capture_count !=0 && (++rate_limit % 500 == 0)) {
for(unsigned i = 0; i < capture_count; i++) {
if (capture_conf[i].valid) {
input_capture_stats_t stats;
stats.chan_in_edges_out = capture_conf[i].chan.channel;
if (ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) {
err(1, "Unable to get stats for chan %u\n", capture_conf[i].chan.channel);
} else {
fprintf(stdout, "FMU: Status chan:%u edges: %d last time:%lld last state:%d overflows:%d lantency:%u\n",
capture_conf[i].chan.channel,
stats.chan_in_edges_out,
stats.last_time,
stats.last_edge,
stats.overflows,
stats.latnecy);
}
}
}
}
/* Check if user wants to quit */
char c;
ret = poll(&fds, 1, 0);
@@ -2398,8 +2678,21 @@ test(void)
}
}
if (capture_count !=0 ) {
for(unsigned i = 0; i < capture_count; i++) {
// Map to channel number
if (capture_conf[i].valid) {
/* Save handler */
if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) {
err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
}
}
}
}
close(fd);
exit(0);
}
@@ -2481,6 +2774,14 @@ fmu_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm4")) {
new_mode = PORT_PWM4;
} else if (!strcmp(verb, "mode_pwm2")) {
new_mode = PORT_PWM2;
} else if (!strcmp(verb, "mode_pwm3")) {
new_mode = PORT_PWM3;
} else if (!strcmp(verb, "mode_pwm3cap1")) {
new_mode = PORT_PWM3CAP1;
} else if (!strcmp(verb, "mode_pwm2cap2")) {
new_mode = PORT_PWM2CAP2;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)