Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 16:49:51 +08:00
Code Issues Actions 11 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-05 23:15:16 +02:00
parent df42d0604e 9804447a66
commit 31850115bb
16 changed files with 1022 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_accel.h
+1 -1
View File
@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_accel);
* ioctl() definitions
*/
#define _ACCELIOCBASE (_SNIOCBASE + 0x20)
#define _ACCELIOCBASE (0x2000)
#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */
apps/drivers/drv_baro.h
+1 -1
View File
@@ -65,7 +65,7 @@ ORB_DECLARE(sensor_baro);
* ioctl() definitions
*/
#define _BAROIOCBASE (_SNIOCBASE + 0x10)
#define _BAROIOCBASE (0x2100)
#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */
apps/drivers/drv_gyro.h
+1 -1
View File
@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_gyro);
* ioctl() definitions
*/
#define _GYROIOCBASE (_SNIOCBASE + 0x10)
#define _GYROIOCBASE (0x2200)
#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */
apps/drivers/drv_mag.h
+1 -1
View File
@@ -75,7 +75,7 @@ ORB_DECLARE(sensor_mag);
* ioctl() definitions
*/
#define _MAGIOCBASE (_SNIOCBASE + 0x30)
#define _MAGIOCBASE (0x2300)
#define _MAGIOC(_n) (_IOC(_MAGIOBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */
apps/drivers/drv_mixer.h
+96
View File
@@ -0,0 +1,96 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file drv_mixer.h
*
* Mixer ioctl interface.
*
* This interface can/should be exported by any device that supports
* control -> actuator mixing.
*/
#ifndef _DRV_MIXER_H
#define _DRV_MIXER_H
#include <stdint.h>
#include <sys/ioctl.h>
#include <systemlib/mixer.h>
/**
* Structure used for receiving mixers.
*
* Note that the mixers array is not actually an array of mixers; it
* simply represents the first mixer in the buffer.
*/
struct MixInfo {
unsigned num_controls;
struct mixer_s mixer;
};
/**
* Handy macro for determining the allocation size of a MixInfo structure.
*/
#define MIXINFO_SIZE(_num_controls) (sizeof(struct MixInfo) + ((_num_controls) * sizeof(struct scaler_s)))
/*
* ioctl() definitions
*/
#define _MIXERIOCBASE (0x2400)
#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n))
/** get the number of actuators that require mixers in *(unsigned)arg */
#define MIXERIOCGETMIXERCOUNT _MIXERIOC(0)
/**
* Copy a mixer from the device into *(struct MixInfo *)arg.
*
* The num_controls field indicates the number of controls for which space
* is allocated following the MixInfo structure. If the allocation
* is too small, no mixer data is retured. The control_count field in
* the MixInfo.mixer structure is always updated.
*
* If no mixer is assigned for the given index, the ioctl returns ENOENT.
*/
#define MIXERIOCGETMIXER(_mixer) _MIXERIOC(0x20 + _mixer)
/**
* Copy a mixer from *(struct mixer_s *)arg to the device.
*
* If arg is zero, the mixer is deleted.
*/
#define MIXERIOCSETMIXER(_mixer) _MIXERIOC(0x40 + _mixer)
#endif /* _DRV_ACCEL_H */
apps/drivers/drv_orb_dev.h
+1 -1
View File
@@ -58,7 +58,7 @@
/** maximum ogbject name length */
#define ORB_MAXNAME 32
#define _ORBIOCBASE (_DIOCBASE + 0x80)
#define _ORBIOCBASE (0x2500)
#define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n))
/*
apps/drivers/drv_pwm_output.h
+1 -1
View File
@@ -94,7 +94,7 @@ ORB_DECLARE(output_pwm);
* Note that ioctls and ObjDev updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _PWM_SERVO_BASE 0x7500
#define _PWM_SERVO_BASE 0x2600
/** arm all servo outputs handle by this driver */
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
apps/px4/fmu/Makefile
+4
View File
@@ -35,4 +35,8 @@
# Interface driver for the PX4FMU board
#
APPNAME = fmu
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/px4/fmu/fmu.cpp
+133 -21
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Driver/configurator for the PX4 FMU multi-purpose port.
* @file fmu.cpp
*
* Driver/configurator for the PX4 FMU multi-purpose port.
*/
#include <nuttx/config.h>
@@ -40,6 +42,7 @@
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
@@ -54,6 +57,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/mixer.h>
@@ -76,6 +80,8 @@ public:
virtual int init();
private:
static const unsigned _max_actuators = 4;
Mode _mode;
int _task;
int _t_actuators;
@@ -85,13 +91,14 @@ private:
volatile bool _task_should_exit;
bool _armed;
MixMixer *_mixer[4];
mixer_s *_mixer[_max_actuators];
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
void task_main();
};
namespace {
namespace
{
FMUServo *g_servo;
@@ -106,17 +113,19 @@ FMUServo::FMUServo(Mode mode) :
_task_should_exit(false),
_armed(false)
{
for (unsigned i = 0; i < 4; i++)
for (unsigned i = 0; i < _max_actuators; i++)
_mixer[i] = nullptr;
}
FMUServo::~FMUServo()
{
if (_task != -1) {
/* task should wake up every 100ms or so at least */
_task_should_exit = true;
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
@@ -142,11 +151,13 @@ FMUServo::init()
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* start the IO interface task */
_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -164,8 +175,6 @@ FMUServo::task_main_trampoline(int argc, char *argv[])
void
FMUServo::task_main()
{
log("ready");
/* configure for PWM output */
switch (_mode) {
case MODE_2PWM:
@@ -173,18 +182,20 @@ FMUServo::task_main()
/* XXX magic numbers */
up_pwm_servo_init(0x3);
break;
case MODE_4PWM:
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
break;
case MODE_NONE:
/* we should never get here... */
break;
}
/* subscribe to objects that we are interested in watching */
_t_actuators = orb_subscribe(ORB_ID(actuator_controls));
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(_t_actuators, 20); /* 50Hz update rate */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
@@ -213,10 +224,11 @@ FMUServo::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
struct actuator_controls ac;
struct actuator_controls_s ac;
float *controls[1] = { &ac.control[0] };
/* get controls */
orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
@@ -225,7 +237,7 @@ FMUServo::task_main()
if (_mixer[i] != nullptr) {
/* mix controls to the actuator */
float output = mixer_mix(_mixer[i], &ac.control[0]);
float output = mixer_mix(_mixer[i], &controls[0]);
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * output));
@@ -235,7 +247,7 @@ FMUServo::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
struct actuator_armed aa;
struct actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -249,7 +261,7 @@ FMUServo::task_main()
::close(_t_armed);
/* make sure servos are off */
up_pwm_servo_deinit();
up_pwm_servo_deinit();
/* note - someone else is responsible for restoring the GPIO config */
@@ -262,6 +274,9 @@ int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
struct MixInfo *mi;
struct mixer_s *mm, *tmm;
switch (cmd) {
case PWM_SERVO_ARM:
@@ -278,15 +293,18 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
if (arg < 2100) {
int channel = cmd - PWM_SERVO_SET(0);
channel = cmd - PWM_SERVO_SET(0);
up_pwm_servo_set(channel, arg);
} else {
ret = -EINVAL;
}
break;
case PWM_SERVO_GET(2):
@@ -295,37 +313,121 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
int channel = cmd - PWM_SERVO_SET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
channel = cmd - PWM_SERVO_SET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
case MIXERIOCGETMIXERCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
break;
case MIXERIOCGETMIXER(3):
case MIXERIOCGETMIXER(2):
if (_mode != MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case MIXERIOCGETMIXER(1):
case MIXERIOCGETMIXER(0):
channel = cmd - MIXERIOCGETMIXER(0);
/* if no mixer is assigned, we return ENOENT */
if (_mixer[channel] == nullptr) {
ret = -ENOENT;
break;
}
/* caller's MixInfo */
mi = (struct MixInfo *)arg;
/* if MixInfo claims to be big enough, copy mixer info */
if (mi->num_controls >= _mixer[channel]->control_count) {
memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count));
} else {
/* just update MixInfo with actual size of the mixer */
mi->mixer.control_count = _mixer[channel]->control_count;
}
break;
case MIXERIOCSETMIXER(3):
case MIXERIOCSETMIXER(2):
if (_mode != MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case MIXERIOCSETMIXER(1):
case MIXERIOCSETMIXER(0):
channel = cmd - MIXERIOCSETMIXER(0);
/* get the caller-supplied mixer and check */
mm = (struct mixer_s *)arg;
if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) { /* only the attitude group is supported */
ret = -EINVAL;
break;
}
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
} else {
tmm = nullptr;
}
/* swap in new mixer for old */
mm = _mixer[channel];
_mixer[channel] = tmm;
/* if there was an old mixer, free it */
if (mm != nullptr)
free(mm);
break;
}
default:
ret = -ENOTTY;
break;
}
return ret;
}
namespace {
namespace
{
enum PortMode {
PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_SERIAL,
PORT_FULL_PWM,
PORT_GPIO_AND_SERIAL,
PORT_PWM_AND_SERIAL,
PORT_PWM_AND_GPIO,
PORT_MODE_UNSET
};
PortMode g_port_mode;
int
fmu_new_mode(PortMode new_mode)
fmu_new_mode(PortMode new_mode)
{
int fd;
int ret = OK;
@@ -334,6 +436,7 @@ fmu_new_mode(PortMode new_mode)
/* get hold of the GPIO configuration descriptor */
fd = open(GPIO_DEVICE_PATH, 0);
if (fd < 0)
return -errno;
@@ -386,15 +489,19 @@ fmu_new_mode(PortMode new_mode)
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0)
ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
close(fd);
/* create new PWM driver if required */
if (servo_mode != FMUServo::MODE_NONE) {
g_servo = new FMUServo(servo_mode);
if (g_servo == nullptr) {
ret = -ENOMEM;
} else {
ret = g_servo->init();
if (ret != OK) {
delete g_servo;
g_servo = nullptr;
@@ -407,7 +514,7 @@ fmu_new_mode(PortMode new_mode)
} // namespace
extern "C" int fmu_main(int argc, char *argv[]);
extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
int
fmu_main(int argc, char *argv[])
@@ -421,14 +528,19 @@ fmu_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(argv[1], "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(argv[1], "mode_pwm")) {
new_mode = PORT_FULL_PWM;
} else if (!strcmp(argv[1], "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(argv[1], "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
apps/systemcmds/mixer/Makefile
+42
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 mixer tool.
#
APPNAME = mixer
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/systemcmds/mixer/mixer.c
+360
View File
@@ -0,0 +1,360 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file mixer.c
*
* Mixer utility.
*/
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <systemlib/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
__EXPORT int mixer_main(int argc, char *argv[]);
static void usage(const char *reason);
static void load(const char *devname, const char *fname);
static void save(const char *devname, const char *fname);
static void show(const char *devname);
int
mixer_main(int argc, char *argv[])
{
if (argc < 2)
usage("missing command");
if (!strcmp(argv[1], "load")) {
if (argc < 4)
usage("missing device or filename");
load(argv[2], argv[3]);
} else if (!strcmp(argv[1], "save")) {
if (argc < 4)
usage("missing device or filename");
save(argv[2], argv[3]);
} else if (!strcmp(argv[1], "show")) {
if (argc < 3)
usage("missing device name");
show(argv[2]);
} else {
usage("unrecognised command");
}
return 0;
}
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer show <device>\n");
fprintf(stderr, " mixer {load|save} <device> [<filename>]\n");
exit(1);
}
static void
load(const char *devname, const char *fname)
{
int defs = -1;
int dev = -1;
unsigned num_mixers = 0;
int ret, result = 1;
struct mixer_s *mixer = NULL;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
}
/* open the definition file */
if ((defs = open(fname, O_RDONLY)) < 0) {
fprintf(stderr, "can't open %s\n", fname);
goto out;
}
/* find out how many mixers the device supports */
ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
if (num_mixers < 1) {
fprintf(stderr, "can't get mixer count from %s\n", devname);
goto out;
}
/* send mixers to the device */
for (unsigned i = 0; i < num_mixers; i++) {
ret = mixer_load(defs, &mixer);
if (ret < 0) {
fprintf(stderr, "read for mixer %d failed\n", i);
goto out;
}
/* end of file? */
if (ret == 0)
break;
if (mixer != NULL) {
/* sanity check the mixer */
ret = mixer_check(mixer, NUM_ACTUATOR_CONTROL_GROUPS, NUM_ACTUATOR_CONTROLS);
if (ret != 0) {
fprintf(stderr, "mixer %u fails sanity check %d\n", i, ret);
goto out;
}
/* send the mixer to the device */
ret = ioctl(dev, MIXERIOCSETMIXER(i), (unsigned long)mixer);
if (ret < 0) {
fprintf(stderr, "mixer %d set failed\n", i);
goto out;
}
free(mixer);
mixer = NULL;
} else {
/* delete the mixer */
ret = ioctl(dev, MIXERIOCSETMIXER(i), 0);
if (ret < 0) {
fprintf(stderr, "mixer %d clear failed\n", i);
goto out;
}
}
}
result = 0;
out:
/* free the mixers array */
if (mixer != NULL)
free(mixer);
if (defs != -1)
close(defs);
if (dev != -1)
close(dev);
exit(result);
}
static int
getmixer(int dev, unsigned mixer_number, struct MixInfo **mip)
{
struct MixInfo *mi = *mip;
int ret;
/* first-round initialisation */
if (mi == NULL) {
mi = (struct MixInfo *)malloc(MIXINFO_SIZE(0));
mi->num_controls = 0;
}
/* loop trying to get the next mixer until the buffer is big enough */
do {
/* try to get the mixer into the buffer as it stands */
ret = ioctl(dev, MIXERIOCGETMIXER(mixer_number), (unsigned long)mi);
if (ret < 0)
return -1;
/* did the mixer fit? */
if (mi->mixer.control_count <= mi->num_controls)
break;
/* re-allocate to suit */
mi->num_controls = mi->mixer.control_count;
mi = (struct MixInfo *)realloc(mi, MIXINFO_SIZE(mi->num_controls));
/* oops, blew up the heap */
if (mi == NULL)
return -1;
} while (true);
*mip = mi;
return 0;
}
static void
save(const char *devname, const char *fname)
{
struct MixInfo *mi = NULL;
int defs = -1;
int dev = -1;
unsigned num_mixers = 0;
int ret, result = 1;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
}
/* find out how many mixers the device supports */
ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
if (num_mixers < 1) {
fprintf(stderr, "can't get mixer count from %s\n", devname);
goto out;
}
/* open the definition file */
if ((defs = open(fname, O_WRONLY | O_CREAT)) < 0) {
fprintf(stderr, "can't open %s\n", fname);
goto out;
}
/* get mixers from the device and save them */
for (unsigned i = 0; i < num_mixers; i++) {
struct mixer_s *mm;
ret = getmixer(dev, i, &mi);
mm = &mi->mixer;
if (ret < 0) {
if (errno != ENOENT)
goto out;
mm = NULL;
}
ret = mixer_save(defs, mm);
if (ret < 0)
goto out;
}
result = 0;
out:
/* free the mixinfo */
if (mi != NULL)
free(mi);
if (defs != -1)
close(defs);
if (dev != -1)
close(dev);
exit(result);
}
static void
show(const char *devname)
{
struct MixInfo *mi = NULL;
int dev = -1;
unsigned num_mixers = 0;
int ret;
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
}
/* find out how many mixers the device supports */
ioctl(dev, MIXERIOCGETMIXERCOUNT, (unsigned long)&num_mixers);
if (num_mixers < 1) {
fprintf(stderr, "can't get mixer count from %s\n", devname);
goto out;
}
/* get mixers from the device and print them */
for (unsigned i = 0; i < num_mixers; i++) {
ret = getmixer(dev, i, &mi);
if (ret < 0) {
if (errno != ENOENT)
goto out;
continue;
}
printf("mixer %d:\n", i);
printf(" -ve scale +ve scale offset low limit high limit\n");
printf("output %8.4f %8.4f %8.4f %8.4f %8.4f\n",
mi->mixer.output_scaler.negative_scale,
mi->mixer.output_scaler.positive_scale,
mi->mixer.output_scaler.offset,
mi->mixer.output_scaler.lower_limit,
mi->mixer.output_scaler.upper_limit);
for (unsigned j = 0; j < mi->mixer.control_count; j++) {
printf("(%u,%u) %8.4f %8.4f %8.4f %8.4f %8.4f\n",
mi->mixer.control_scaler[j].control_group,
mi->mixer.control_scaler[j].control_index,
mi->mixer.control_scaler[j].negative_scale,
mi->mixer.control_scaler[j].positive_scale,
mi->mixer.control_scaler[j].offset,
mi->mixer.control_scaler[j].lower_limit,
mi->mixer.control_scaler[j].upper_limit);
}
}
out:
/* free the mixinfo */
if (mi != NULL)
free(mi);
if (dev != -1)
close(dev);
exit(0);
}
apps/systemlib/mixer.c
+247 -33
View File
@@ -43,55 +43,97 @@
* See mixer.h for more details.
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "mixer.h"
static int
scale_check(struct MixScaler *scale)
scale_check(struct scaler_s *scale)
{
if (scale->offset > 1.0f)
return -1;
if (scale->offset > 1.0f)
return -1;
if (scale->lower_limit > scale->upper_limit)
return -1;
if (scale->lower_limit < -1.0f)
return -1;
if (scale->upper_limit > 1.0f)
return -1;
return 0;
}
if (scale->offset > 1.1f)
return 1;
int
mixer_check(struct MixMixer *mixer, unsigned control_count)
{
if (mixer->control_count < 1)
return -1;
if (mixer->control_count > control_count)
return -1;
if (!scale_check(&mixer->output_scaler))
return -1;
if (scale->offset < -1.1f)
return 2;
if (scale->lower_limit > scale->upper_limit)
return 3;
if (scale->lower_limit < -1.1f)
return 4;
if (scale->upper_limit > 1.1f)
return 5;
for (unsigned i = 0; i < mixer->control_count; i++) {
if (mixer->control_scaler[i].control >= control_count)
return -1;
if (!scale_check(&mixer->control_scaler[i]))
return -1;
}
return 0;
}
int
mixer_check(struct mixer_s *mixer, unsigned group_count, unsigned control_count)
{
int ret;
/* sanity that presumes that a mixer includes a control no more than once */
if (mixer->control_count > (group_count * control_count))
return -2;
/* validate the output scaler */
ret = scale_check(&mixer->output_scaler);
if (ret != 0)
return ret;
/* validate input scalers */
for (unsigned i = 0; i < mixer->control_count; i++) {
/* range-check input controls */
if (mixer->control_scaler[i].control_group >= group_count)
return -3;
if (mixer->control_scaler[i].control_index >= control_count)
return -3;
/* validate the scaler */
ret = scale_check(&mixer->control_scaler[i]);
if (ret != 0)
return (10 * i + ret);
}
return 0;
}
void
mixer_requires(struct mixer_s *mixer, uint32_t *groups)
{
for (unsigned i = 0; i < mixer->control_count; i++)
*groups |= 1 << mixer->control_scaler[i].control_group;
}
/**
* Apply a scaler to a value.
*
* @param scaler The applied scaler.
* @param input The value to scale.
* @output The scaled value.
*/
static float
scale(struct MixScaler *scaler, float input)
scale(struct scaler_s *scaler, float input)
{
float output;
if (input < 0.0f) {
output = (input * scaler->negative_scale) + scaler->offset;
} else {
output = (input * scaler->positive_scale) + scaler->offset;
}
if (output > scaler->upper_limit) {
output = scaler->upper_limit;
} else if (output < scaler->lower_limit) {
output = scaler->lower_limit;
}
@@ -100,15 +142,187 @@ scale(struct MixScaler *scaler, float input)
}
float
mixer_mix(struct MixMixer *mixer, float *controls)
mixer_mix(struct mixer_s *mixer, float **controls)
{
struct MixScaler *scaler;
float sum = 0.0f;
for (unsigned i = 0; i < mixer->control_count; i++) {
scaler = &mixer->control_scaler[i];
sum += scale(scaler, controls[scaler->control]);
struct scaler_s *scaler = &mixer->control_scaler[i];
float *cg = controls[scaler->control_group];
sum += scale(scaler, cg[scaler->control_index]);
}
return scale(&mixer->output_scaler, sum);
}
/**
* Effectively fdgets()
*/
static int
mixer_getline(int fd, char *line, unsigned maxlen)
{
int ret;
char c;
while (--maxlen) {
ret = read(fd, &c, 1);
if (ret <= 0)
return ret;
if (c == '\r')
continue;
if (c == '\n') {
*line = '\0';
return 1;
}
*line++ = c;
}
/* line too long */
puts("line too long");
return -1;
}
static int
mixer_load_scaler(const char *buf, struct scaler_s *scaler)
{
unsigned u[2];
int s[5];
if (sscanf(buf, "S: %u %u %d %d %d %d %d",
&u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7)
return -1;
scaler->control_group = u[0];
scaler->control_index = u[1];
scaler->negative_scale = s[0] / 10000.0f;
scaler->positive_scale = s[1] / 10000.0f;
scaler->offset = s[2] / 10000.0f;
scaler->lower_limit = s[3] / 10000.0f;
scaler->upper_limit = s[4] / 10000.0f;
return 0;
}
int
mixer_load(int fd, struct mixer_s **mp)
{
int ret, result = -1;
struct mixer_s *mixer = NULL;
char buf[100];
unsigned scalers;
ret = mixer_getline(fd, buf, sizeof(buf));
/* end of file? */
if (ret == 0)
result = 0;
/* can't proceed */
if (ret < 1)
goto out;
/* get header */
if (sscanf(buf, "M: %u", &scalers) != 1)
goto out;
/* if there are scalers, load them */
if (scalers > 0) {
/* allocate mixer */
scalers--;
mixer = (struct mixer_s *)malloc(MIXER_SIZE(scalers));
if (mixer == NULL)
goto out;
mixer->control_count = scalers;
ret = mixer_getline(fd, buf, sizeof(buf));
if (ret < 1)
goto out;
if (mixer_load_scaler(buf, &mixer->output_scaler))
goto out;
for (unsigned i = 0; i < scalers; i++) {
ret = mixer_getline(fd, buf, sizeof(buf));
if (ret < 1)
goto out;
if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
goto out;
}
} else {
/* we return NULL for the mixer, which is interpreted elsewhere as "no mixer" */
}
result = 1;
out:
/* on error, discard allocated mixer */
if ((result <= 0) && (mixer != NULL))
free(mixer);
*mp = mixer;
return result;
}
static int
mixer_save_scaler(char *buf, struct scaler_s *scaler)
{
int s[5];
s[0] = 10000.0f * scaler->negative_scale;
s[1] = 10000.0f * scaler->positive_scale;
s[2] = 10000.0f * scaler->offset;
s[3] = 10000.0f * scaler->lower_limit;
s[4] = 10000.0f * scaler->upper_limit;
return sprintf(buf, "S: %u %u %d %d %d %d %d\n",
scaler->control_group, scaler->control_index,
s[0], s[1], s[2], s[3], s[4]);
}
int
mixer_save(int fd, struct mixer_s *mixer)
{
char buf[100];
int len, ret;
/* write the mixer header */
len = sprintf(buf, "M: %u\n", (mixer != NULL) ? mixer->control_count : 0);
ret = write(fd, buf, len);
if (ret != len)
return -1;
if (mixer != NULL) {
/* write the output scaler */
len = mixer_save_scaler(buf, &mixer->output_scaler);
write(fd, buf, len);
if (ret != len)
return -1;
/* write the control scalers */
for (unsigned j = 0; j < mixer->control_count; j++) {
len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
write(fd, buf, len);
if (ret != len)
return -1;
}
}
return 0;
}
apps/systemlib/mixer.h
+104 -33
View File
@@ -31,9 +31,12 @@
*
****************************************************************************/
#ifndef _SYSTEMLIB_MIXER_H
#define _SYSTEMLIB_MIXER_H
/**
* @file mixer.h
*
*
* Generic control value mixing library.
*
* This library implements a generic mixer function that can be used
@@ -43,10 +46,13 @@
* Terminology
* ===========
*
* control
* control value
* A mixer input value, typically provided by some controlling
* component of the system.
*
* control group
* A collection of controls provided by a single controlling component.
*
* actuator
* The mixer output value.
*
@@ -55,7 +61,7 @@
*
* An actuator derives its value from the combination of one or more
* control values. Each of the control values is scaled according to
* the actuator's configuration and then combined to produce the
* the actuator's configuration and then combined to produce the
* actuator value, which may then be further scaled to suit the specific
* output type.
*
@@ -100,7 +106,7 @@
* Mixing
* ------
*
* Mixing is performed by summing the scaled control values.
* Mixing is performed by summing the scaled control values.
*
*
* Controls
@@ -109,7 +115,7 @@
* Each mixer is presented with an array of controls from which it
* selects the set that will be mixed for each actuator.
*
* The precise assignment of controls may vary depending on the
* The precise assignment of controls may vary depending on the
* application, but the following assignments should be used
* when appropriate.
*
@@ -121,45 +127,110 @@
* 3 | primary thrust
*/
struct MixScaler
{
unsigned control; /**< control consumed by this scaler */
float negative_scale; /**< scale for inputs < 0 */
float positive_scale; /**< scale for inputs > 0 */
float offset; /**< bias applied to output */
float lower_limit; /**< minimum output value */
float upper_limit; /**< maximum output value */
};
struct scaler_s {
float negative_scale; /**< scale for inputs < 0 */
float positive_scale; /**< scale for inputs > 0 */
float offset; /**< bias applied to output */
float lower_limit; /**< minimum output value */
float upper_limit; /**< maximum output value */
uint8_t control_group; /**< control group this scaler reads from */
uint8_t control_index; /**< control index within the group */
};
struct MixMixer
{
unsigned control_count; /**< number of control scalers */
struct MixScaler output_scaler; /**< scaler applied to mixer output */
struct MixScaler control_scaler[0]; /**< array of control scalers */
};
struct mixer_s {
unsigned control_count; /**< number of control scalers */
struct scaler_s output_scaler; /**< scaler applied to mixer output */
struct scaler_s control_scaler[0]; /**< array of control scalers */
};
/**
* Handy macro for determining the allocation size of a mixer.
*/
#define MIXER_SIZE(_num_scalers) (sizeof(struct mixer_s) + ((_num_scalers) * sizeof(struct scaler_s)))
__BEGIN_DECLS
/**
* Perform a mixer calculation.
*
* Note that the controls array is assumed to be sufficiently large for any control
* index in the mixer.
* Note that the controls array, and the arrays it indexes, are assumed
* to be sufficiently large for any control index in the mixer.
*
* @param mixer Mixer configuration.
* @param controls Array of input control values.
* @param controls Array of pointers to control group values.
* @return The mixed output.
*/
__EXPORT float mixer_mix(struct MixMixer *mixer, float *controls);
__EXPORT float mixer_mix(struct mixer_s *mixer, float **controls);
/**
* Check a mixer configuration for sanity.
*
* @param mixer The mixer configuration to be checked.
* @param group_count The highest-numbered control group that
* should be considered legal.
* @param control_count The highest control index that should be
* considered legal.
* @return Zero if the mixer configuration is sane,
* nonzero otherwise.
*/
__EXPORT int mixer_check(struct mixer_s *mixer, unsigned group_count, unsigned control_count);
/**
* Evaluate the control inputs to a mixer and update the bitmask of
* required control groups.
*
* This function allows an actuator driver to selectively fetch just
* the control groups required to support a particular mixer or set of
* mixers.
*
* @param mixer The mixer being evaluated.
* @param groups Pointer to a bitmask to be updated with set bits
* corresponding to the control groups used by the
* mixer.
*/
__EXPORT void mixer_requires(struct mixer_s *mixer, uint32_t *groups);
/**
* Read a mixer definition from a file.
*
* A mixer definition is a text representation of the configuration of a
* mixer. The definition consists of a single-line header indicating the
* number of scalers and then one line defining each scaler. The first
* scaler in the file is always the output scaler, followed by the input
* scalers.
*
* M: <scaler count>
* S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
* S: ...
*
* The <control ...> values for the output scaler are ignored by the mixer.
*
* Values marked * are integers representing floating point values; values are
* scaled by 10000 on load/save.
*
* Multiple mixer definitions may be stored in a single file; it is assumed that
* the reader will know how many to expect and read accordingly.
*
* A mixer entry with a scaler count of zero indicates a disabled mixer. This
* will return NULL for the mixer when processed by this function, and will be
* generated by passing NULL as the mixer to mixer_save.
*
* @param fd The file to read the definitions from.
* @param mixer Mixer is returned here.
* @return 1 if a mixer was read, zero on EOF or negative on error.
*/
__EXPORT int mixer_load(int fd, struct mixer_s **mixer);
/**
* Save a mixer definition to a file.
*
* @param fd The file to write the definitions to.
* @param mixer The mixer definition to save.
* @return Zero on success, negative on error.
*/
__EXPORT int mixer_save(int fd, struct mixer_s *mixers);
/**
* Check a mixer configuration for sanity.
*
* @param mixer The mixer configuration to be checked.
* @param control_count The number of controls in the system.
* @return Zero if the mixer configuration is sane,
* nonzero otherwise.
*/
__EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count);
__END_DECLS
#endif /* _SYSTEMLIB_MIXER_H */
apps/uORB/objects_common.cpp
+10 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Common object definitions without a better home.
* @file objects_common.h
*
* Common object definitions without a better home.
*/
#include <nuttx/config.h>
@@ -99,3 +101,10 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
#include "topics/vehicle_attitude_setpoint.h"
ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
#include "topics/actuator_controls.h"
ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
apps/uORB/topics/actuator_controls.h
+19 -8
View File
@@ -32,11 +32,15 @@
****************************************************************************/
/**
* @file Actuator control topic - mixer inputs.
* @file actuator_controls.h
*
* Values published to this topic are the outputs of the vehicle control
* Actuator control topics - mixer inputs.
*
* Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
*
* Each topic can be published by a single controller
*/
#ifndef TOPIC_ACTUATOR_CONTROLS_H
@@ -45,17 +49,24 @@
#include <stdint.h>
#include "../uORB.h"
#define NUM_ACTUATOR_CONTROLS 16
#define NUM_ACTUATOR_CONTROLS 8
#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
struct actuator_controls
{
struct actuator_controls_s {
float control[NUM_ACTUATOR_CONTROLS];
};
ORB_DECLARE(actuator_controls);
/* actuator control sets; this list can be expanded as more controllers emerge */
ORB_DECLARE(actuator_controls_0);
ORB_DECLARE(actuator_controls_1);
ORB_DECLARE(actuator_controls_2);
ORB_DECLARE(actuator_controls_3);
struct actuator_armed
{
/* control sets with pre-defined applications */
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
/** global 'actuator output is live' control. */
struct actuator_armed_s {
bool armed;
};
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -48,6 +48,7 @@ CONFIGURED_APPS += systemcmds/reboot
CONFIGURED_APPS += systemcmds/perf
CONFIGURED_APPS += systemcmds/top
CONFIGURED_APPS += systemcmds/boardinfo
CONFIGURED_APPS += systemcmds/mixer
#CONFIGURED_APPS += systemcmds/calibration
CONFIGURED_APPS += uORB