dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Move PWM rate configuration, etc. into a separate utility and out of the individual drivers.

This commit is contained in:
px4dev 2013-03-12 00:40:22 -07:00
parent 57429fd12c
commit 7011fe563b
6 changed files with 289 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
apps/drivers/hil/hil.cpp
View File

@ -655,7 +655,7 @@ enum PortMode {
PortMode g_port_mode;
int
hil_new_mode(PortMode new_mode, int update_rate)
hil_new_mode(PortMode new_mode)
{
// uint32_t gpio_bits;
@ -713,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_hil->set_mode(servo_mode);
if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0))
g_hil->set_pwm_rate(update_rate);
return OK;
}
@ -800,7 +798,6 @@ int
hil_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNDEFINED;
unsigned pwm_rate = 0;
const char *verb = argv[1];
if (hil_start() != OK)
@ -812,22 +809,6 @@ hil_main(int argc, char *argv[])
// this was all cut-and-pasted from the FMU driver; it's junk
if (!strcmp(verb, "mode_pwm")) {
int ch;
while ((ch = getopt(argc - 1, argv + 1, "u:")) != EOF) {
switch (ch) {
case 'u':
pwm_rate = strtol(optarg, nullptr, 0);
break;
case ':':
errx(1, "missing parameter");
default:
errx(1, "unrecognised option");
}
}
new_mode = PORT1_FULL_PWM;
} else if (!strcmp(verb, "mode_pwm_serial")) {
@ -854,7 +835,7 @@ hil_main(int argc, char *argv[])
return OK;
/* switch modes */
return hil_new_mode(new_mode, pwm_rate);
return hil_new_mode(new_mode);
}
if (!strcmp(verb, "test"))
@ -865,6 +846,6 @@ hil_main(int argc, char *argv[])
fprintf(stderr, "HIL: unrecognized command, try:\n");
fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
return -EINVAL;
}

56
apps/drivers/px4fmu/fmu.cpp
View File

@ -309,6 +309,8 @@ PX4FMU::set_mode(Mode mode)
int
PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
{
debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
for (unsigned pass = 0; pass < 2; pass++) {
for (unsigned group = 0; group < _max_actuators; group++) {
@ -323,17 +325,22 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
warn("rate group %u mask %x bad overlap %x", group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK)
if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
warn("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK)
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed");
return -EINVAL;
}
}
}
}
@ -428,6 +435,7 @@ PX4FMU::task_main()
_current_update_rate = 50;
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
orb_set_interval(_t_actuators, update_rate_in_ms);
}
@ -643,6 +651,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
@ -1023,9 +1032,6 @@ int
fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
unsigned pwm_rate = 0;
uint32_t alt_channels = 0;
bool alt_channels_set = false;
const char *verb = argv[1];
if (fmu_start() != OK)
@ -1041,29 +1047,6 @@ fmu_main(int argc, char *argv[])
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(verb, "mode_pwm")) {
int ch;
char *ap;
while ((ch = getopt(argc - 1, argv + 1, "c:u:")) != EOF) {
switch (ch) {
case 'u':
pwm_rate = strtol(optarg, nullptr, 0);
break;
case 'c':
alt_channels = strtol(optarg, &ap, 0);
if (*ap == '\0')
alt_channels_set = true;
break;
case ':':
errx(1, "missing parameter");
default:
errx(1, "unrecognised option");
}
}
new_mode = PORT_FULL_PWM;
} else if (!strcmp(verb, "mode_gpio_serial")) {
@ -1084,19 +1067,8 @@ fmu_main(int argc, char *argv[])
return OK;
/* switch modes */
return fmu_new_mode(new_mode);
/* if new modes are PWM modes, respect the -u and -c options */
if ((new_mode == PORT_FULL_PWM) || (new_mode == PORT_PWM_AND_GPIO)) {
if (pwm_rate != 0) {
if (g_fmu->set_pwm_alt_rate(pwm_rate) != OK)
errx(1, "error setting alternate PWM rate");
}
if (alt_channels_set) {
if (g_fmu->set_pwm_alt_channels(alt_channels))
errx(1, "serror setting alternate PWM rate channels");
}
}
int ret = fmu_new_mode(new_mode);
exit(ret == OK ? 0 : 1);
}
if (!strcmp(verb, "test"))
@ -1106,6 +1078,6 @@ fmu_main(int argc, char *argv[])
fake(argc - 1, argv + 1);
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_alt_rate_in_hz] [-c pwm_alt_rate_channel_mask], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
exit(1);
}

50
apps/drivers/px4io/px4io.cpp
View File

@ -1501,6 +1501,26 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
{
void
start(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(1, "already loaded");
/* create the driver - it will set g_dev */
(void)new PX4IO();
if (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->init()) {
delete g_dev;
errx(1, "driver init failed");
}
exit(0);
}
void
test(void)
{
@ -1595,34 +1615,8 @@ px4io_main(int argc, char *argv[])
if (argc < 2)
goto out;
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr)
errx(1, "already loaded");
/* create the driver - it will set g_dev */
(void)new PX4IO();
if (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->init()) {
delete g_dev;
errx(1, "driver init failed");
}
/* look for the optional pwm update rate for the supported modes */
if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) {
if (argc > 2 + 1) {
#warning implement this
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
return 1;
}
}
exit(0);
}
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
if (!strcmp(argv[1], "recovery")) {

42
apps/systemcmds/pwm/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Build the pwm tool.
#
APPNAME = pwm
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

207
apps/systemcmds/pwm/pwm.c Normal file
View File

@ -0,0 +1,207 @@
/****************************************************************************
*
* Copyright (C) 2013 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 pwm.c
*
* PWM servo output configuration and monitoring tool.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
#include "drivers/drv_pwm_output.h"
static void usage(const char *reason);
__EXPORT int pwm_main(int argc, char *argv[]);
static void
usage(const char *reason)
{
if (reason != NULL)
warnx("%s", reason);
errx(1,
"usage:\n"
"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <alt_channel_mask>] [arm|disarm] [<channel_value> ...]\n"
" -v Print information about the PWM device\n"
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" <alt_rate> PWM update rate for channels in <alt_channel_mask>\n"
" <alt_channel_mask> Bitmask of channels to update at the alternate rate\n"
" arm | disarm Arm or disarm the ouptut\n"
" <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n");
}
int
pwm_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
unsigned alt_rate = 0;
uint32_t alt_channels;
bool alt_channels_set = false;
bool print_info = false;
int ch;
int ret;
char *ep;
while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
switch (ch) {
case 'c':
alt_channels = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_channel_mask value");
alt_channels_set = true;
break;
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
case 'v':
print_info = true;
break;
default:
usage(NULL);
}
}
argc -= optind;
argv += optind;
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* assign alternate rate to channels */
if (alt_channels_set) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, alt_channels);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE (check mask vs. device capabilities)");
}
/* iterate remaining arguments */
unsigned channel = 0;
while (argc--) {
const char *arg = argv[0];
argv++;
if (!strcmp(arg, "arm")) {
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
}
if (!strcmp(arg, "disarm")) {
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
continue;
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", channel);
channel++;
continue;
}
usage("unrecognised option");
}
/* print verbose info */
if (print_info) {
/* get the number of servo channels */
unsigned count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
/* print current servo values */
printf("PWM output values:\n");
for (unsigned i = 0; i < count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("%u: %uus\n", i, spos);
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
printf("Available alt_channel_mask groups:\n");
for (unsigned i = 0; i < count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
break;
if (group_mask != 0)
printf(" 0x%x", group_mask);
}
printf("\n");
fflush(stdout);
}
exit(0);
}

1
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -67,6 +67,7 @@ CONFIGURED_APPS += systemcmds/boardinfo
CONFIGURED_APPS += systemcmds/mixer
CONFIGURED_APPS += systemcmds/eeprom
CONFIGURED_APPS += systemcmds/param
CONFIGURED_APPS += systemcmds/pwm
CONFIGURED_APPS += systemcmds/bl_update
CONFIGURED_APPS += systemcmds/preflight_check
CONFIGURED_APPS += systemcmds/delay_test