Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 10:50:19 +08:00
Code Issues Actions 23 Packages Projects Releases Wiki Activity

Merged with master

Browse Source
This commit is contained in:
Lorenz Meier
2013-01-07 17:50:49 +01:00
parent e4bfd78847 659543f852
commit 41f657e3e5
54 changed files with 2150 additions and 905 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -11,6 +11,7 @@ Make.dep
*.o
*.a
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
@@ -40,3 +41,4 @@ nsh_romfsimg.h
cscope.out
.configX-e
nuttx-export.zip
dot.gdbinit
ROMFS/scripts/rc.PX4IO
+22 -24
View File
@@ -1,12 +1,9 @@
#!nsh
#
# Flight startup script for PX4FMU with PX4IO carrier board.
#
echo "[init] doing PX4IO startup..."
set USB no
#
# Start the ORB
# Start the object request broker
#
uorb start
@@ -20,15 +17,27 @@ then
param load
fi
#
# Enable / connect to PX4IO
#
px4io start
#
# Load an appropriate mixer. FMU_pass.mix is a passthru mixer
# which is good for testing. See ROMFS/mixers for a full list of mixers.
#
mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink
# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable)
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start the commander.
@@ -41,35 +50,24 @@ commander start
attitude_estimator_ekf start
#
# Configure PX4FMU for operation with PX4IO
# Start the attitude and position controller
#
# XXX arguments?
#
px4fmu start
fixedwing_att_control start
fixedwing_pos_control start
#
# Start the fixed-wing controller
#
fixedwing_control start
#
# Fire up the PX4IO interface.
#
px4io start
#
# Start looking for a GPS.
# Start GPS capture. Comment this out if you do not have a GPS.
#
gps start
#
# Start logging to microSD if we can
#
#
sh /etc/init.d/rc.logging
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry (dumb).
# use the same UART for telemetry
#
echo "[init] startup done, exiting."
echo "[init] startup done"
exit
ROMFS/scripts/rc.sensors
+1
View File
@@ -8,6 +8,7 @@
#
ms5611 start
adc start
if mpu6000 start
then
apps/Makefile
+3
View File
@@ -112,6 +112,9 @@ endef
$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
# EXTERNAL_APPS is used to add out of tree apps to the build
INSTALLED_APPS += $(EXTERNAL_APPS)
# The external/ directory may also be added to the INSTALLED_APPS. But there
# is no external/ directory in the repository. Rather, this directory may be
# provided by the user (possibly as a symbolic link) to add libraries and
apps/commander/commander.c
+18 -8
View File
@@ -70,6 +70,7 @@
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
@@ -1262,6 +1263,11 @@ int commander_thread_main(int argc, char *argv[])
struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd));
/* subscribe to battery topic */
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
// uint8_t vehicle_state_previous = current_status.state_machine;
float voltage_previous = 0.0f;
@@ -1300,15 +1306,19 @@ int commander_thread_main(int argc, char *argv[])
handle_command(stat_pub, &current_status, &cmd);
}
battery_voltage = sensors.battery_voltage_v;
battery_voltage_valid = sensors.battery_voltage_valid;
orb_check(battery_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
battery_voltage = battery.voltage_v;
battery_voltage_valid = true;
/*
* Only update battery voltage estimate if voltage is
* valid and system has been running for two and a half seconds
*/
if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
bat_remain = battery_remaining_estimate_voltage(battery_voltage);
/*
* Only update battery voltage estimate if system has
* been running for two and a half seconds.
*/
if (hrt_absolute_time() - start_time > 2500000) {
bat_remain = battery_remaining_estimate_voltage(battery_voltage);
}
}
/* Slow but important 8 Hz checks */
apps/drivers/boards/px4fmu/px4fmu_init.c
+5 -20
View File
@@ -95,8 +95,6 @@
* Protected Functions
****************************************************************************/
extern int adc_devinit(void);
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -209,8 +207,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
@@ -233,23 +230,11 @@ __EXPORT int nsh_archinitialize(void)
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
#endif /* SPI3 */
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK) {
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK) {
/* Give up */
message("[boot] FAILED adc_devinit: %d\n", adc_state);
return -ENODEV;
}
}
#endif
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
//stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available?
//stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
return OK;
}
apps/drivers/boards/px4io/px4io_init.c
+3
View File
@@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ACC_OC_DETECT);
stm32_configgpio(GPIO_SERVO_OC_DETECT);
stm32_configgpio(GPIO_BTN_SAFETY);
stm32_configgpio(GPIO_ADC_VBATT);
stm32_configgpio(GPIO_ADC_IN5);
}
apps/drivers/boards/px4io/px4io_internal.h
+5 -22
View File
@@ -61,28 +61,6 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* R/C in/out channels **************************************************************/
/* XXX just GPIOs for now - eventually timer pins */
#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
/* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@@ -98,3 +76,8 @@
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
/* Analog inputs ********************************************************************/
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
apps/drivers/drv_adc.h
+52
View File
@@ -0,0 +1,52 @@
/****************************************************************************
*
* 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_adc.h
*
* ADC driver interface.
*
* This defines additional operations over and above the standard NuttX
* ADC API.
*/
#pragma once
#include <stdint.h>
#include <sys/ioctl.h>
#define ADC_DEVICE_PATH "/dev/adc0"
/*
* ioctl definitions
*/
apps/drivers/drv_mixer.h
+4 -19
View File
@@ -100,28 +100,13 @@ struct mixer_simple_s {
*/
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */
struct mixer_rotor_output_s {
float angle; /**< rotor angle clockwise from forward in radians */
float distance; /**< motor distance from centre in arbitrary units */
};
/** multirotor mixer */
struct mixer_multirotor_s {
uint8_t rotor_count;
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
};
/* _MIXERIOC(3) was deprecated */
/* _MIXERIOC(4) was deprecated */
/**
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg
* Add mixer(s) from the buffer in (const char *)arg
*/
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
/**
* Add mixers(s) from a the file in (const char *)arg
*/
#define MIXERIOCLOADFILE _MIXERIOC(4)
#define MIXERIOCLOADBUF _MIXERIOC(5)
/*
* XXX Thoughts for additional operations:
apps/drivers/drv_pwm_output.h
+3
View File
@@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** set update rate in Hz */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
apps/drivers/hil/hil.cpp
+12 -15
View File
@@ -503,6 +503,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
// up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@@ -577,26 +581,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@@ -605,10 +602,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
default:
ret = -ENOTTY;
break;
apps/drivers/px4fmu/fmu.cpp
+11 -17
View File
@@ -470,6 +470,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@@ -500,7 +504,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
channel = cmd - PWM_SERVO_SET(0);
channel = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
@@ -544,28 +548,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@@ -574,7 +569,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
apps/drivers/px4io/px4io.cpp
+174 -75
View File
@@ -61,6 +61,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@@ -73,6 +74,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
#include <px4io/protocol.h>
#include "uploader.h"
@@ -91,7 +93,7 @@ public:
bool dump_one;
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@@ -109,13 +111,19 @@ private:
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
orb_advert_t _to_battery; ///< battery status / voltage
battery_status_s _battery_status;///< battery status data
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
const char *volatile _mix_buf; ///< mixer text buffer
volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
bool _primary_pwm_device; ///< true if we are the default PWM output
uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
@@ -157,6 +165,11 @@ private:
*/
void config_send();
/**
* Send a buffer containing mixer text to PX4IO
*/
int mixer_send(const char *buf, unsigned buflen);
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
@@ -186,8 +199,10 @@ PX4IO::PX4IO() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_mix_buf(nullptr),
_mix_buf_len(0),
_primary_pwm_device(false),
_relays(0),
_switch_armed(false),
_send_needed(false),
_config_needed(false)
@@ -208,6 +223,7 @@ PX4IO::~PX4IO()
/* give it another 100ms */
usleep(100000);
}
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
@@ -238,6 +254,7 @@ PX4IO::init()
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -249,13 +266,16 @@ PX4IO::init()
debug("PX4IO connected");
break;
}
usleep(100000);
}
if (!_connected) {
/* error here will result in everything being torn down */
log("PX4IO not responding");
return -EIO;
}
return OK;
}
@@ -268,7 +288,7 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void
PX4IO::task_main()
{
log("starting");
debug("starting");
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@@ -290,10 +310,12 @@ PX4IO::task_main()
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream");
goto out;
}
hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"),
@@ -321,6 +343,10 @@ PX4IO::task_main()
memset(&_input_rc, 0, sizeof(_input_rc));
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
/* do not advertise the battery status until its clear that a battery is connected */
memset(&_battery_status, 0, sizeof(_battery_status));
_to_battery = -1;
/* poll descriptor */
pollfd fds[3];
fds[0].fd = _serial_fd;
@@ -330,13 +356,18 @@ PX4IO::task_main()
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
log("ready");
debug("ready");
/* lock against the ioctl handler */
lock();
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
lock();
/* this would be bad... */
if (ret < 0) {
@@ -353,26 +384,21 @@ PX4IO::task_main()
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
/* if we have new control data from the ORB, handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* scale controls to PWM (temporary measure) */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _controls.control[i]);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
/* and flag for update */
_send_needed = true;
}
/* if we have an arming state update, handle it */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
@@ -391,14 +417,26 @@ PX4IO::task_main()
_config_needed = false;
config_send();
}
/* send a mixer update if needed */
if (_mix_buf != nullptr) {
mixer_send(_mix_buf, _mix_buf_len);
/* clear the buffer record so the ioctl handler knows we're done */
_mix_buf = nullptr;
_mix_buf_len = 0;
}
}
unlock();
out:
debug("exiting");
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_serial_fd);
/* clean up the alternate device node */
@@ -451,25 +489,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
lock();
// lock();
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
_connected = true;
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
for (int i = 0; i < rep->channel_count; i++) {
_input_rc.values[i] = rep->rc_channel[i];
}
@@ -479,6 +519,23 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
/* remember the latched arming switch state */
_switch_armed = rep->armed;
/* publish battery information */
/* only publish if battery has a valid minimum voltage */
if (rep->battery_mv > 3300) {
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = rep->battery_mv / 1000.0f;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* announce the battery voltage if needed, just publish else */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
_send_needed = true;
/* if monitoring, dump the received info */
@@ -486,13 +543,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not");
for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]);
printf("\n");
}
out:
unlock();
// unlock();
return;
}
void
@@ -505,17 +565,21 @@ PX4IO::io_send()
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
cmd.output_control[i] = _outputs.output[i];
/* publish as we send */
/* XXX needs to be based off post-mix values from the IO side */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
// XXX relays
/* update relays */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
/* armed and not locked down */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
debug("send error %d", ret);
}
@@ -529,10 +593,46 @@ PX4IO::config_send()
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret)
debug("config error %d", ret);
}
int
PX4IO::mixer_send(const char *buf, unsigned buflen)
{
uint8_t frame[HX_STREAM_MAX_FRAME];
px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
msg->action = F2I_MIXER_ACTION_RESET;
do {
unsigned count = buflen;
if (count > F2I_MIXER_MAX_TEXT)
count = F2I_MIXER_MAX_TEXT;
if (count > 0) {
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
}
int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
if (ret) {
log("mixer send error %d", ret);
return ret;
}
msg->action = F2I_MIXER_ACTION_APPEND;
} while (buflen > 0);
return 0;
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
@@ -554,9 +654,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
_send_needed = true;
break;
case PWM_SERVO_SET_UPDATE_RATE:
// not supported yet
ret = -EINVAL;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
/* fake an update to the selected 'servo' channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
@@ -572,68 +677,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case GPIO_RESET:
_relays = 0;
_send_needed = true;
break;
case GPIO_SET:
case GPIO_CLEAR:
/* make sure only valid bits are being set */
if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
ret = EINVAL;
break;
}
if (cmd == GPIO_SET) {
_relays |= arg;
} else {
_relays &= ~arg;
}
_send_needed = true;
break;
case GPIO_GET:
*(uint32_t *)arg = _relays;
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
ret = 0; /* load always resets */
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
case MIXERIOCLOADBUF:
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* set the buffer up for transfer */
_mix_buf = (const char *)arg;
_mix_buf_len = strnlen(_mix_buf, 1024);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
/* drop the lock and wait for the thread to clear the transmit */
unlock();
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
while (_mix_buf != nullptr)
usleep(1000);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
lock();
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
ret = 0;
break;
default:
@@ -675,6 +765,13 @@ test(void)
close(fd);
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
orb_advertise(ORB_ID(actuator_armed), &aa);
exit(0);
}
@@ -694,6 +791,7 @@ monitor(void)
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
exit(0);
}
@@ -780,6 +878,7 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "monitor"))
monitor();
apps/drivers/px4io/uploader.cpp
+1
View File
@@ -190,6 +190,7 @@ PX4IO_Uploader::drain()
do {
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}
apps/drivers/stm32/adc/Makefile
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# 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.
#
############################################################################
#
# STM32 ADC driver
#
APPNAME = adc
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
include $(APPDIR)/mk/app.mk
apps/drivers/stm32/adc/adc.cpp
+387
View File
@@ -0,0 +1,387 @@
/****************************************************************************
*
* 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 adc.cpp
*
* Driver for the STM32 ADC.
*
* This is a low-rate driver, designed for sampling things like voltages
* and so forth. It avoids the gross complexity of the NuttX ADC driver.
*/
#include <nuttx/config.h>
#include <drivers/device/device.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_adc.h>
#include <arch/stm32/chip.h>
#include <stm32_internal.h>
#include <stm32_gpio.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
/*
* Register accessors.
* For now, no reason not to just use ADC1.
*/
#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
#define rSR REG(STM32_ADC_SR_OFFSET)
#define rCR1 REG(STM32_ADC_CR1_OFFSET)
#define rCR2 REG(STM32_ADC_CR2_OFFSET)
#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
#define rHTR REG(STM32_ADC_HTR_OFFSET)
#define rLTR REG(STM32_ADC_LTR_OFFSET)
#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
#define rDR REG(STM32_ADC_DR_OFFSET)
#ifdef STM32_ADC_CCR
# define rCCR REG(STM32_ADC_CCR_OFFSET)
#endif
class ADC : public device::CDev
{
public:
ADC(uint32_t channels);
~ADC();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t read(file *filp, char *buffer, size_t len);
protected:
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
private:
static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */
hrt_call _call;
perf_counter_t _sample_perf;
unsigned _channel_count;
adc_msg_s *_samples; /**< sample buffer */
/** work trampoline */
static void _tick_trampoline(void *arg);
/** worker function */
void _tick();
/**
* Sample a single channel and return the measured value.
*
* @param channel The channel to sample.
* @return The sampled value, or 0xffff if
* sampling failed.
*/
uint16_t _sample(unsigned channel);
};
ADC::ADC(uint32_t channels) :
CDev("adc", ADC_DEVICE_PATH),
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
_channel_count(0),
_samples(nullptr)
{
_debug_enabled = true;
/* always enable the temperature sensor */
channels |= 1 << 16;
/* allocate the sample array */
for (unsigned i = 0; i < 32; i++) {
if (channels & (1 << i)) {
_channel_count++;
}
}
_samples = new adc_msg_s[_channel_count];
/* prefill the channel numbers in the sample array */
if (_samples != nullptr) {
unsigned index = 0;
for (unsigned i = 0; i < 32; i++) {
if (channels & (1 << i)) {
_samples[index].am_channel = i;
_samples[index].am_data = 0;
index++;
}
}
}
}
ADC::~ADC()
{
if (_samples != nullptr)
delete _samples;
}
int
ADC::init()
{
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_CAL;
usleep(100);
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
usleep(10);
rCR2 |= ADC_CR2_ADON;
usleep(10);
rCR2 |= ADC_CR2_ADON;
usleep(10);
/* kick off a sample and wait for it to complete */
hrt_abstime now = hrt_absolute_time();
rCR2 |= ADC_CR2_SWSTART;
while (!(rSR & ADC_SR_EOC)) {
/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
if ((hrt_absolute_time() - now) > 500) {
log("sample timeout");
return -1;
return 0xffff;
}
}
debug("init done");
/* create the device node */
return CDev::init();
}
int
ADC::ioctl(file *filp, int cmd, unsigned long arg)
{
return -ENOTTY;
}
ssize_t
ADC::read(file *filp, char *buffer, size_t len)
{
const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
if (len > maxsize)
len = maxsize;
/* block interrupts while copying samples to avoid racing with an update */
irqstate_t flags = irqsave();
memcpy(buffer, _samples, len);
irqrestore(flags);
return len;
}
int
ADC::open_first(struct file *filp)
{
/* get fresh data */
_tick();
/* and schedule regular updates */
hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
return 0;
}
int
ADC::close_last(struct file *filp)
{
hrt_cancel(&_call);
return 0;
}
void
ADC::_tick_trampoline(void *arg)
{
((ADC *)arg)->_tick();
}
void
ADC::_tick()
{
/* scan the channel set and sample each */
for (unsigned i = 0; i < _channel_count; i++)
_samples[i].am_data = _sample(_samples[i].am_channel);
}
uint16_t
ADC::_sample(unsigned channel)
{
perf_begin(_sample_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
rCR2 |= ADC_CR2_SWSTART;
/* wait for the conversion to complete */
hrt_abstime now = hrt_absolute_time();
while (!(rSR & ADC_SR_EOC)) {
/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
if ((hrt_absolute_time() - now) > 50) {
log("sample timeout");
return 0xffff;
}
}
/* read the result and clear EOC */
uint16_t result = rDR;
perf_end(_sample_perf);
return result;
}
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int adc_main(int argc, char *argv[]);
namespace
{
ADC *g_adc;
void
test(void)
{
int fd = open(ADC_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "can't open ADC device");
for (unsigned i = 0; i < 50; i++) {
adc_msg_s data[8];
ssize_t count = read(fd, data, sizeof(data));
if (count < 0)
errx(1, "read error");
unsigned channels = count / sizeof(data[0]);
for (unsigned j = 0; j < channels; j++) {
printf ("%d: %u ", data[j].am_channel, data[j].am_data);
}
printf("\n");
usleep(500000);
}
exit(0);
}
}
int
adc_main(int argc, char *argv[])
{
if (g_adc == nullptr) {
/* XXX this hardcodes the minimum channel set for PX4FMU - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11));
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");
if (g_adc->init() != OK) {
delete g_adc;
errx(1, "ADC init failed");
}
}
if (argc > 1) {
if (!strcmp(argv[1], "test"))
test();
}
exit(0);
}
apps/mavlink/mavlink_log.c
+3 -2
View File
@@ -40,6 +40,7 @@
*/
#include <string.h>
#include <stdlib.h>
#include "mavlink_log.h"
@@ -51,7 +52,7 @@ void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) {
}
int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) {
return lb->count == lb->size;
return lb->count == (int)lb->size;
}
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) {
@@ -77,4 +78,4 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
} else {
return 1;
}
}
}
apps/px4io/Makefile
+3 -1
View File
@@ -41,7 +41,9 @@
#
CSRCS = $(wildcard *.c) \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c
../systemlib/perf_counter.c \
../systemlib/up_cxxinitialize.c
CXXSRCS = $(wildcard *.cpp)
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
apps/px4io/adc.c
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* 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 adc.c
*
* Simple ADC support for PX4IO on STM32.
*/
#include <nuttx/config.h>
#include <stdint.h>
#include <nuttx/arch.h>
#include <arch/stm32/chip.h>
#include <stm32_internal.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#define DEBUG
#include "px4io.h"
/*
* Register accessors.
* For now, no reason not to just use ADC1.
*/
#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
#define rSR REG(STM32_ADC_SR_OFFSET)
#define rCR1 REG(STM32_ADC_CR1_OFFSET)
#define rCR2 REG(STM32_ADC_CR2_OFFSET)
#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
#define rHTR REG(STM32_ADC_HTR_OFFSET)
#define rLTR REG(STM32_ADC_LTR_OFFSET)
#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
#define rDR REG(STM32_ADC_DR_OFFSET)
perf_counter_t adc_perf;
int
adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
up_udelay(1);
if (rCR2 & ADC_CR2_RSTCAL)
return -1;
rCR2 |= ADC_CR2_CAL;
up_udelay(100);
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
return 0;
}
uint16_t
adc_measure(unsigned channel)
{
perf_begin(adc_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
rCR2 |= ADC_CR2_ADON;
/* wait for the conversion to complete */
hrt_abstime now = hrt_absolute_time();
while (!(rSR & ADC_SR_EOC)) {
/* never spin forever - this will give a bogus result though */
if ((hrt_absolute_time() - now) > 1000) {
debug("adc timeout");
break;
}
}
/* read the result and clear EOC */
uint16_t result = rDR;
perf_end(adc_perf);
return result;
}
apps/px4io/comms.c
+91 -21
View File
@@ -61,8 +61,7 @@
#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
int frame_rx;
int frame_bad;
#define FMU_STATUS_INTERVAL 1000000 /* 100ms */
static int fmu_fd;
static hx_stream_t stream;
@@ -70,6 +69,8 @@ static struct px4io_report report;
static void comms_handle_frame(void *arg, const void *buffer, size_t length);
perf_counter_t comms_rx_errors;
static void
comms_init(void)
{
@@ -77,6 +78,9 @@ comms_init(void)
fmu_fd = open("/dev/ttyS1", O_RDWR);
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
/* default state in the report to FMU */
report.i2f_magic = I2F_MAGIC;
@@ -87,6 +91,9 @@ comms_init(void)
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(fmu_fd, TCSANOW, &t);
/* init the ADC */
adc_init();
}
void
@@ -110,6 +117,7 @@ comms_main(void)
if (fds.revents & POLLIN) {
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]);
}
@@ -123,7 +131,8 @@ comms_main(void)
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
@@ -132,12 +141,59 @@ comms_main(void)
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i];
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
report.battery_mv = system_state.battery_mv;
report.adc_in = system_state.adc_in5;
report.overcurrent = system_state.overcurrent;
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
/*
* Fetch ADC values, check overcurrent flags, etc.
*/
static hrt_abstime last_status_time;
if ((now - last_status_time) > FMU_STATUS_INTERVAL) {
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* V counts
* 5 1001
* 6 1219
* 7 1436
* 8 1653
* 9 1870
* 10 2086
* 11 2303
* 12 2522
* 13 2738
* 14 2956
* 15 3172
* 16 3389
*
* slope = 0.0046067
* intercept = 0.3863
*
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
system_state.battery_mv = (4150 + (counts * 46)) / 10;
system_state.adc_in5 = adc_measure(ADC_IN5);
system_state.overcurrent =
(OVERCURRENT_SERVO ? (1 << 0) : 0) |
(OVERCURRENT_ACC ? (1 << 1) : 0);
last_status_time = now;
}
}
}
@@ -146,12 +202,10 @@ comms_handle_config(const void *buffer, size_t length)
{
const struct px4io_config *cfg = (struct px4io_config *)buffer;
if (length != sizeof(*cfg)) {
frame_bad++;
if (length != sizeof(*cfg))
return;
}
frame_rx++;
/* XXX */
}
static void
@@ -159,40 +213,51 @@ comms_handle_command(const void *buffer, size_t length)
{
const struct px4io_command *cmd = (struct px4io_command *)buffer;
if (length != sizeof(*cmd)) {
frame_bad++;
if (length != sizeof(*cmd))
return;
}
frame_rx++;
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->servo_command[i];
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->output_control[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
if(system_state.arm_ok && !cmd->arm_ok) {
if (system_state.arm_ok && !cmd->arm_ok)
system_state.armed = false;
}
system_state.arm_ok = cmd->arm_ok;
system_state.mixer_use_fmu = true;
system_state.fmu_data_received = true;
/* handle changes signalled by FMU */
// if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false;
/* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
/* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
if (system_state.relays[i] != cmd->relay_state[i]) {
switch (i) {
case 0:
POWER_ACC1(cmd->relay_state[i]);
break;
case 1:
POWER_ACC2(cmd->relay_state[i]);
break;
case 2:
POWER_RELAY1(cmd->relay_state[i]);
break;
case 3:
POWER_RELAY2(cmd->relay_state[i]);
break;
}
}
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
@@ -205,11 +270,16 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
case F2I_MAGIC:
comms_handle_command(buffer, length);
break;
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
default:
frame_bad++;
break;
}
}
apps/px4io/controls.c
+3 -1
View File
@@ -90,6 +90,7 @@ controls_main(void)
if (fds[0].revents & POLLIN)
locked |= dsm_input();
if (fds[1].revents & POLLIN)
locked |= sbus_input();
@@ -139,6 +140,7 @@ ppm_input(void)
/* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels;
for (unsigned i = 0; i < ppm_decoded_channels; i++)
system_state.rc_channel_data[i] = ppm_buffer[i];
@@ -150,5 +152,5 @@ ppm_input(void)
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
}
}
}
apps/px4io/dsm.c
+26 -14
View File
@@ -47,7 +47,7 @@
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#define DEBUG
@@ -97,6 +97,7 @@ dsm_init(const char *device)
dsm_guess_format(true);
debug("DSM: ready");
} else {
debug("DSM: open failed");
}
@@ -118,7 +119,7 @@ dsm_input(void)
* frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@@ -126,6 +127,7 @@ dsm_input(void)
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) {
dsm_frame_drops++;
@@ -142,6 +144,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
last_rx_time = now;
/*
@@ -153,7 +156,7 @@ dsm_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < DSM_FRAME_SIZE)
goto out;
goto out;
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -164,7 +167,7 @@ dsm_input(void)
out:
/*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
}
@@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel);
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel);
@@ -222,7 +226,7 @@ dsm_guess_format(bool reset)
if (samples++ < 5)
return;
/*
/*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
*
@@ -233,7 +237,7 @@ dsm_guess_format(bool reset)
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static uint32_t masks[] = {
static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
@@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
if (cs10 == masks[i])
votes10++;
if (cs11 == masks[i])
votes11++;
}
if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11;
debug("DSM: detected 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10;
debug("DSM: detected 10-bit format");
@@ -270,13 +277,13 @@ static void
dsm_decode(hrt_abstime frame_time)
{
/*
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
*/
/*
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
@@ -292,7 +299,7 @@ dsm_decode(hrt_abstime frame_time)
}
/*
* The encoding of the first two bytes is uncertain, so we're
* The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
*
* Each channel is a 16-bit unsigned value containing either a 10-
@@ -322,9 +329,10 @@ dsm_decode(hrt_abstime frame_time)
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11)
value /= 2;
value += 998;
/*
/*
* Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have.
*
@@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
case 0:
channel = 2;
break;
case 1:
channel = 0;
break;
case 2:
channel = 1;
default:
break;
}
system_state.rc_channel_data[channel] = value;
}
apps/px4io/mixer.c → apps/px4io/mixer.cpp
+104 -29
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file mixer.c
* @file mixer.cpp
*
* Control channel input/output mixer and failsafe.
*/
@@ -49,8 +49,12 @@
#include <fcntl.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
//#define DEBUG
#include "px4io.h"
}
/*
* Count of periodic calls in which we have no FMU input.
@@ -58,28 +62,23 @@
static unsigned fmu_input_drops;
#define FMU_INPUT_DROP_LIMIT 20
/*
* Update a mixer based on the current control signals.
*/
static void mixer_update(int mixer, uint16_t *inputs, int input_count);
/* current servo arm/disarm state */
bool mixer_servos_armed = false;
/*
* Each mixer consumes a set of inputs and produces a single output.
*/
struct mixer {
uint16_t current_value;
/* XXX more config here */
} mixers[IO_SERVO_COUNT];
/* selected control values and count for mixing */
static uint16_t *control_values;
static int control_count;
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control);
static MixerGroup mixer_group(mixer_callback, 0);
void
mixer_tick(void)
{
uint16_t *control_values;
int control_count;
int i;
bool should_arm;
/*
@@ -87,7 +86,7 @@ mixer_tick(void)
*/
if (system_state.mixer_use_fmu) {
/* we have recent control data from the FMU */
control_count = PX4IO_OUTPUT_CHANNELS;
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* check that we are receiving fresh data from the FMU */
@@ -98,6 +97,7 @@ mixer_tick(void)
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false;
}
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
@@ -115,17 +115,31 @@ mixer_tick(void)
}
/*
* Tickle each mixer, if we have control data.
* Run the mixers if we have any control data at all.
*/
if (control_count > 0) {
for (i = 0; i < IO_SERVO_COUNT; i++) {
mixer_update(i, control_values, control_count);
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
if (i < mixed) {
/* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
} else {
/* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0;
}
/*
* If we are armed, update the servo output.
*/
if (system_state.armed && system_state.arm_ok)
up_pwm_servo_set(i, mixers[i].current_value);
up_pwm_servo_set(i, system_state.servos[i]);
}
}
@@ -133,6 +147,7 @@ mixer_tick(void)
* Decide whether the servos should be armed right now.
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@@ -145,13 +160,73 @@ mixer_tick(void)
}
}
static void
mixer_update(int mixer, uint16_t *inputs, int input_count)
static int
mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control)
{
/* simple passthrough for now */
if (mixer < input_count) {
mixers[mixer].current_value = inputs[mixer];
} else {
mixers[mixer].current_value = 0;
}
/* if the control index refers to an input that's not valid, we can't return it */
if (control_index >= control_count)
return -1;
/* scale from current PWM units (1000-2000) to mixer input values */
control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
return 0;
}
static char mixer_text[256];
static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
px4io_mixdata *msg = (px4io_mixdata *)buffer;
debug("mixer text %u", length);
if (length < sizeof(px4io_mixdata))
return;
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
debug("reset");
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
debug("append %d", length);
/* check for overflow - this is really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
return;
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
debug("buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
debug("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
}
break;
}
}
apps/px4io/protocol.h
+43 -13
View File
@@ -41,31 +41,27 @@
#pragma once
#define PX4IO_OUTPUT_CHANNELS 8
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1)
/* command from FMU to IO */
/**
* Periodic command from FMU to IO.
*/
struct px4io_command {
uint16_t f2i_magic;
#define F2I_MAGIC 0x636d
#define F2I_MAGIC 0x636d
uint16_t servo_command[PX4IO_OUTPUT_CHANNELS];
uint16_t output_control[PX4IO_CONTROL_CHANNELS];
bool relay_state[PX4IO_RELAY_CHANNELS];
bool arm_ok;
};
/* config message from FMU to IO */
struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
/* XXX currently nothing here */
};
/* report from IO to FMU */
/**
* Periodic report from IO to FMU
*/
struct px4io_report {
uint16_t i2f_magic;
#define I2F_MAGIC 0x7570
@@ -73,6 +69,40 @@ struct px4io_report {
uint16_t rc_channel[PX4IO_INPUT_CHANNELS];
bool armed;
uint8_t channel_count;
uint16_t battery_mv;
uint16_t adc_in;
uint8_t overcurrent;
};
/**
* As-needed config message from FMU to IO
*/
struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
/* XXX currently nothing here */
};
/**
* As-needed mixer data upload.
*
* This message adds text to the mixer text buffer; the text
* buffer is drained as the definitions are consumed.
*/
struct px4io_mixdata {
uint16_t f2i_mixer_magic;
#define F2I_MIXER_MAGIC 0x6d74
uint8_t action;
#define F2I_MIXER_ACTION_RESET 0
#define F2I_MIXER_ACTION_APPEND 1
char text[0]; /* actual text size may vary */
};
/* maximum size is limited by the HX frame size */
#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
#pragma pack(pop)
apps/px4io/px4io.c
+6 -1
View File
@@ -55,10 +55,15 @@
__EXPORT int user_start(int argc, char *argv[]);
extern void up_cxxinitialize(void);
struct sys_state_s system_state;
int user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
@@ -83,7 +88,7 @@ int user_start(int argc, char *argv[])
up_pwm_servo_init(0xff);
/* start the flight control signal handler */
task_create("FCon",
task_create("FCon",
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)controls_main,
apps/px4io/px4io.h
+41 -18
View File
@@ -31,11 +31,11 @@
*
****************************************************************************/
/**
* @file px4io.h
*
* General defines and structures for the PX4IO module firmware.
*/
/**
* @file px4io.h
*
* General defines and structures for the PX4IO module firmware.
*/
#include <nuttx/config.h>
@@ -66,8 +66,7 @@
/*
* System state structure.
*/
struct sys_state_s
{
struct sys_state_s {
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
@@ -82,7 +81,12 @@ struct sys_state_s
/*
* Control signals from FMU.
*/
uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS];
/*
* Mixed servo outputs
*/
uint16_t servos[IO_SERVO_COUNT];
/*
* Relay controls
@@ -105,17 +109,25 @@ struct sys_state_s
bool fmu_data_received;
/*
* Current serial interface mode, per the serial_rx_mode parameter
* in the config packet.
* Measured battery voltage in mV
*/
uint8_t serial_rx_mode;
uint16_t battery_mv;
/*
* ADC IN5 measurement
*/
uint16_t adc_in5;
/*
* Power supply overcurrent status bits.
*/
uint8_t overcurrent;
};
extern struct sys_state_s system_state;
extern int frame_rx;
extern int frame_bad;
#if 0
/*
* Software countdown timers.
*
@@ -127,6 +139,7 @@ extern int frame_bad;
#define TIMER_SANITY 7
#define TIMER_NUM_TIMERS 8
extern volatile int timers[TIMER_NUM_TIMERS];
#endif
/*
* GPIO handling.
@@ -136,19 +149,23 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s))
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s))
#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT)
#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT
#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
#define ADC_VBATT 4
#define ADC_IN5 5
/*
* Mixer
*/
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
/*
* Safety switch/LED.
@@ -160,6 +177,12 @@ extern void safety_init(void);
*/
extern void comms_main(void) __attribute__((noreturn));
/*
* Sensors/misc inputs
*/
extern int adc_init(void);
extern uint16_t adc_measure(unsigned channel);
/*
* R/C receiver handling.
*/
apps/px4io/safety.c
+16 -6
View File
@@ -31,9 +31,9 @@
*
****************************************************************************/
/**
* @file Safety button logic.
*/
/**
* @file Safety button logic.
*/
#include <nuttx/config.h>
#include <stdio.h>
@@ -95,13 +95,13 @@ safety_init(void)
static void
safety_check_button(void *arg)
{
/*
/*
* Debounce the safety button, change state if it has been held for long enough.
*
*/
safety_button_pressed = BUTTON_SAFETY;
if(safety_button_pressed) {
if (safety_button_pressed) {
//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
}
@@ -109,34 +109,42 @@ safety_check_button(void *arg)
if (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */
system_state.armed = true;
counter++;
system_state.fmu_report_due = true;
}
/* Disarm quickly */
/* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) {
if (counter < DISARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == DISARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
system_state.armed = false;
counter++;
system_state.fmu_report_due = true;
}
} else {
counter = 0;
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) {
if (system_state.arm_ok) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
}
@@ -167,8 +175,10 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
failsafe = !failsafe;
} else {
failsafe = false;
}
LED_AMBER(failsafe);
}
apps/px4io/sbus.c
+42 -32
View File
@@ -53,7 +53,7 @@
#include "debug.h"
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_INPUT_CHANNELS 18
static int sbus_fd = -1;
@@ -87,9 +87,9 @@ sbus_init(const char *device)
partial_frame_count = 0;
last_rx_time = hrt_absolute_time();
debug("Sbus: ready");
debug("S.Bus: ready");
} else {
debug("Sbus: open failed");
debug("S.Bus: open failed");
}
return sbus_fd;
@@ -109,7 +109,7 @@ sbus_input(void)
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@@ -117,6 +117,7 @@ sbus_input(void)
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
@@ -133,6 +134,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
last_rx_time = now;
/*
@@ -144,7 +146,7 @@ sbus_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
goto out;
goto out;
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -155,7 +157,7 @@ sbus_input(void)
out:
/*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
}
@@ -177,23 +179,23 @@ struct sbus_bit_pick {
uint8_t mask;
uint8_t lshift;
};
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} }
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
static void
@@ -205,16 +207,20 @@ sbus_decode(hrt_abstime frame_time)
return;
}
/* if the failsafe bit is set, we consider the frame invalid */
if (frame[23] & (1 << 4)) {
return;
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
if ((frame[23] & (1 << 2)) && /* signal lost */
(frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */
system_state.rc_channels = 0;
return 1;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
@@ -232,20 +238,24 @@ sbus_decode(hrt_abstime frame_time)
value |= piece;
}
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998;
}
if (PX4IO_INPUT_CHANNELS >= 18) {
chancount = 18;
/* XXX decode the two switch channels */
/* decode switch channels if data fields are wide enough */
if (chancount > 17) {
/* channel 17 (index 16) */
system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
system_state.rc_channels = chancount;
/* and note that we have received data from the R/C controller */
system_state.rc_channels_timestamp = frame_time;
system_state.rc_channels_timestamp = frame_time;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
apps/sdlog/sdlog.c
+1 -1
View File
@@ -554,7 +554,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
.vbat = buf.raw.battery_voltage_v,
.vbat = 0.0f, /* XXX use battery_status uORB topic */
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
apps/sensors/sensors.cpp
+25 -12
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
@@ -156,10 +157,12 @@ private:
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
orb_advert_t _rc_pub; /**< raw r/c control topic */
orb_advert_t _battery_pub; /**< battery status */
perf_counter_t _loop_perf; /**< loop performance counter */
struct rc_channels_s _rc; /**< r/c channel data */
struct battery_status_s _battery_status; /**< battery status */
struct {
float min[_rc_max_chan_count];
@@ -348,6 +351,7 @@ Sensors::Sensors() :
_sensor_pub(-1),
_manual_control_pub(-1),
_rc_pub(-1),
_battery_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -636,8 +640,8 @@ Sensors::adc_init()
_fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
if (_fd_adc < 0) {
warn("/dev/adc0");
errx(1, "FATAL: no ADC found");
warnx("/dev/adc0");
warnx("FATAL: no ADC found");
}
}
@@ -844,14 +848,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
/* Voltage in volts */
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling);
if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
raw.battery_voltage_valid = false;
raw.battery_voltage_v = 0.f;
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
} else {
raw.battery_voltage_valid = true;
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* announce the battery voltage if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
raw.battery_voltage_counter++;
@@ -879,7 +891,7 @@ Sensors::ppm_poll()
*/
if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
for (int i = 0; i < ppm_decoded_channels; i++) {
for (unsigned i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
}
@@ -1039,12 +1051,13 @@ Sensors::task_main()
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
raw.battery_voltage_v = BAT_VOL_INITIAL;
raw.adc_voltage_v[0] = 0.9f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
raw.battery_voltage_counter = 0;
raw.battery_voltage_valid = false;
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
/* get a set of initial values */
accel_poll(raw);
apps/systemcmds/mixer/mixer.c
+49 -25
View File
@@ -43,14 +43,17 @@
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <ctype.h>
#include <nuttx/compiler.h>
#include <systemlib/err.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 usage(const char *reason) noreturn_function;
static void load(const char *devname, const char *fname) noreturn_function;
int
mixer_main(int argc, char *argv[])
@@ -63,12 +66,9 @@ mixer_main(int argc, char *argv[])
usage("missing device or filename");
load(argv[2], argv[3]);
} else {
usage("unrecognised command");
}
return 0;
usage("unrecognised command");
}
static void
@@ -79,34 +79,58 @@ usage(const char *reason)
fprintf(stderr, "usage:\n");
fprintf(stderr, " mixer load <device> <filename>\n");
/* XXX automatic setups for quad, etc. */
/* XXX other useful commands? */
exit(1);
}
static void
load(const char *devname, const char *fname)
{
int dev = -1;
int ret, result = 1;
int dev;
FILE *fp;
char line[80];
char buf[512];
/* open the device */
if ((dev = open(devname, 0)) < 0) {
fprintf(stderr, "can't open %s\n", devname);
goto out;
if ((dev = open(devname, 0)) < 0)
err(1, "can't open %s\n", devname);
/* reset mixers on the device */
if (ioctl(dev, MIXERIOCRESET, 0))
err(1, "can't reset mixers on %s", devname);
/* open the mixer definition file */
fp = fopen(fname, "r");
if (fp == NULL)
err(1, "can't open %s", fname);
/* read valid lines from the file into a buffer */
buf[0] = '\0';
for (;;) {
/* get a line, bail on error/EOF */
line[0] = '\0';
if (fgets(line, sizeof(line), fp) == NULL)
break;
/* if the line doesn't look like a mixer definition line, skip it */
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
continue;
/* XXX an optimisation here would be to strip extra whitespace */
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
break;
/* add the line to the buffer */
strcat(buf, line);
}
/* tell it to load the file */
ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
/* XXX pass the buffer to the device */
int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
if (ret != 0) {
fprintf(stderr, "failed loading %s\n", fname);
}
result = 0;
out:
if (dev != -1)
close(dev);
exit(result);
if (ret < 0)
err(1, "error loading mixers from %s", fname);
exit(0);
}
apps/systemlib/mixer/mixer.cpp
+7 -81
View File
@@ -137,89 +137,15 @@ NullMixer::groups_required(uint32_t &groups)
}
/****************************************************************************/
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
mixer_simple_s *mixinfo) :
Mixer(control_cb, cb_handle),
_info(mixinfo)
NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
}
NullMixer *nm = nullptr;
SimpleMixer::~SimpleMixer()
{
if (_info != nullptr)
free(_info);
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_info == nullptr)
return 0;
if (space < 1)
return 0;
for (unsigned i = 0; i < _info->control_count; i++) {
float input;
_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
input);
sum += scale(_info->controls[i].scaler, input);
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
buflen -= 2;
}
*outputs = scale(_info->output_scaler, sum);
return 1;
}
void
SimpleMixer::groups_required(uint32_t &groups)
{
for (unsigned i = 0; i < _info->control_count; i++)
groups |= 1 << _info->controls[i].control_group;
}
int
SimpleMixer::check()
{
int ret;
float junk;
/* sanity that presumes that a mixer includes a control no more than once */
/* max of 32 groups due to groups_required API */
if (_info->control_count > 32)
return -2;
/* validate the output scaler */
ret = scale_check(_info->output_scaler);
if (ret != 0)
return ret;
/* validate input scalers */
for (unsigned i = 0; i < _info->control_count; i++) {
/* verify that we can fetch the control */
if (_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
junk) != 0) {
return -3;
}
/* validate the scaler */
ret = scale_check(_info->controls[i].scaler);
if (ret != 0)
return (10 * i + ret);
}
return 0;
return nm;
}
apps/systemlib/mixer/mixer.h
+116 -30
View File
@@ -234,50 +234,51 @@ public:
void add_mixer(Mixer *mixer);
/**
* Reads a mixer definition from a file and configures a corresponding
* group.
* Remove all the mixers from the group.
*/
void reset();
/**
* Adds mixers to the group based on a text description in a buffer.
*
* The mixer group must be empty when this function is called.
* Mixer definitions begin with a single capital letter and a colon.
* The actual format of the mixer definition varies with the individual
* mixers; they are summarised here, but see ROMFS/mixers/README for
* more details.
*
* A mixer definition is a text representation of the configuration of a
* mixer. Definition lines begin with a capital letter followed by a colon.
* Null Mixer
* ..........
*
* Null Mixer:
* The null mixer definition has the form:
*
* Z:
* Z:
*
* This mixer generates a constant zero output, and is normally used to
* skip over outputs that are not in use.
* Simple Mixer
* ............
*
* Simple Mixer:
* A simple mixer definition begins with:
*
* M: <scaler count>
* S: <control group> <control index> <negative_scale*> <positive_scale*> <offset*> <lower_limit*> <upper_limit*>
* S: ...
* M: <control count>
* O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* 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.
* The definition continues with <control count> entries describing the control
* inputs and their scaling, in the form:
*
* The <control ...> values for the output scaler are ignored by the mixer.
* S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
*
* Multirotor Mixer
* ................
*
* The multirotor mixer definition is a single line of the form:
*
* Values marked * are integers representing floating point values; values are
* scaled by 10000 on load/save.
* R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
*
* 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 path The mixer configuration file to read.
* @param buf The mixer configuration buffer.
* @param buflen The length of the buffer, updated to reflect
* bytes as they are consumed.
* @return Zero on successful load, nonzero otherwise.
*/
int load_from_file(const char *path);
int load_from_buf(const char *buf, unsigned &buflen);
private:
Mixer *_first; /**< linked list of mixers */
@@ -294,6 +295,21 @@ public:
NullMixer();
~NullMixer() {};
/**
* Factory method.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new NullMixer instance, or nullptr
* if the text format is bad.
*/
static NullMixer *from_text(const char *buf, unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
};
@@ -318,6 +334,47 @@ public:
mixer_simple_s *mixinfo);
~SimpleMixer();
/**
* Factory method with full external configuration.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new SimpleMixer instance, or nullptr
* if the text format is bad.
*/
static SimpleMixer *from_text(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
const char *buf,
unsigned &buflen);
/**
* Factory method for PWM/PPM input to internal float representation.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param input The control index used when fetching the input.
* @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out)
* @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out)
* @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out)
* @return A new SimpleMixer instance, or nullptr if one could not be
* allocated.
*/
static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
unsigned input,
uint16_t min,
uint16_t mid,
uint16_t max);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
@@ -330,10 +387,18 @@ public:
* @return Zero if the mixer makes sense, nonzero otherwise.
*/
int check();
protected:
private:
mixer_simple_s *_info;
mixer_simple_s *_info;
static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
static int parse_control_scaler(const char *buf,
unsigned &buflen,
mixer_scaler_s &scaler,
uint8_t &control_group,
uint8_t &control_index);
};
/**
@@ -395,6 +460,27 @@ public:
float deadband);
~MultirotorMixer();
/**
* Factory method.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new MultirotorMixer instance, or nullptr
* if the text format is bad.
*/
static MultirotorMixer *from_text(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
const char *buf,
unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
apps/systemlib/mixer/mixer_group.cpp
+61 -280
View File
@@ -56,250 +56,6 @@
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
namespace
{
/**
* Effectively fdgets() with some extra smarts.
*/
static int
mixer_getline(int fd, char *line, unsigned maxlen)
{
/* reduce line budget by 1 to account for terminal NUL */
maxlen--;
/* loop looking for a non-comment line */
for (;;) {
int ret;
char c;
char *p = line;
/* loop reading characters for this line */
for (;;) {
ret = read(fd, &c, 1);
/* on error or EOF, return same */
if (ret <= 0) {
debug("read: EOF");
return ret;
}
/* ignore carriage returns */
if (c == '\r')
continue;
/* line termination */
if (c == '\n') {
/* ignore malformed lines */
if ((p - line) < 4)
break;
if (line[1] != ':')
break;
/* terminate line as string and return */
*p = '\0';
debug("read: '%s'", line);
return 1;
}
/* if we have space, accumulate the byte and go on */
if ((p - line) < maxlen)
*p++ = c;
}
}
}
/**
* Parse an output scaler from the buffer.
*/
static int
mixer_parse_output_scaler(const char *buf, mixer_scaler_s &scaler)
{
int s[5];
if (sscanf(buf, "O: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4]) != 5) {
debug("scaler parse failed on '%s'", buf);
return -1;
}
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
scaler.offset = s[2] / 10000.0f;
scaler.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
/**
* Parse a control scaler from the buffer.
*/
static int
mixer_parse_control_scaler(const char *buf, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
{
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) {
debug("scaler parse failed on '%s'", buf);
return -1;
}
control_group = u[0];
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.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
SimpleMixer *
mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, unsigned inputs)
{
mixer_simple_s *mixinfo = nullptr;
char buf[60];
int ret;
/* let's assume we're going to read a simple mixer */
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
mixinfo->control_count = inputs;
/* first, get the output scaler */
ret = mixer_getline(fd, buf, sizeof(buf));
if (ret < 1) {
debug("failed reading for output scaler");
goto fail;
}
if (mixer_parse_output_scaler(buf, mixinfo->output_scaler)) {
debug("failed parsing output scaler");
goto fail;
}
/* now get any inputs */
for (unsigned i = 0; i < inputs; i++) {
ret = mixer_getline(fd, buf, sizeof(buf));
if (ret < 1) {
debug("failed reading for control scaler");
goto fail;
}
if (mixer_parse_control_scaler(buf,
mixinfo->controls[i].scaler,
mixinfo->controls[i].control_group,
mixinfo->controls[i].control_index)) {
debug("failed parsing control scaler");
goto fail;
}
debug("got control %d", i);
}
/* XXX should be a factory that validates the mixinfo ... */
return new SimpleMixer(control_cb, cb_handle, mixinfo);
fail:
free(mixinfo);
return nullptr;
}
MultirotorMixer *
mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf)
{
MultirotorMixer::Geometry geometry;
char geomname[8];
int s[4];
if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) {
debug("multirotor parse failed on '%s'", buf);
return nullptr;
}
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorMixer::QUAD_X;
} else if (!strcmp(geomname, "6+")) {
geometry = MultirotorMixer::HEX_PLUS;
} else if (!strcmp(geomname, "6x")) {
geometry = MultirotorMixer::HEX_X;
} else if (!strcmp(geomname, "8+")) {
geometry = MultirotorMixer::OCTA_PLUS;
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
} else {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
}
return new MultirotorMixer(
control_cb,
cb_handle,
geometry,
s[0] / 10000.0f,
s[1] / 10000.0f,
s[2] / 10000.0f,
s[3] / 10000.0f);
}
int
mixer_load(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd, Mixer *&mixer)
{
int ret;
char buf[60];
unsigned inputs;
ret = mixer_getline(fd, buf, sizeof(buf));
/* end of file or error ?*/
if (ret < 1) {
debug("getline %d", ret);
return ret;
}
/* slot is empty - allocate a null mixer */
if (buf[0] == 'Z') {
debug("got null mixer");
mixer = new NullMixer();
return 1;
}
/* is it a simple mixer? */
if (sscanf(buf, "M: %u", &inputs) == 1) {
debug("got simple mixer with %d inputs", inputs);
mixer = mixer_load_simple(control_cb, cb_handle, fd, inputs);
return (mixer == nullptr) ? -1 : 1;
}
/* is it a multirotor mixer? */
if (buf[0] == 'R') {
debug("got a multirotor mixer");
mixer = mixer_load_multirotor(control_cb, cb_handle, buf);
return (mixer == nullptr) ? -1 : 1;
}
/* we don't recognise the mixer type */
debug("unrecognized mixer type '%c'", buf[0]);
return -1;
}
} // namespace
MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
Mixer(control_cb, cb_handle),
_first(nullptr)
@@ -308,14 +64,7 @@ MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) :
MixerGroup::~MixerGroup()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
reset();
}
void
@@ -332,6 +81,19 @@ MixerGroup::add_mixer(Mixer *mixer)
mixer->_next = nullptr;
}
void
MixerGroup::reset()
{
Mixer *mixer;
/* discard sub-mixers */
while (_first != nullptr) {
mixer = _first;
_first = mixer->_next;
delete mixer;
}
}
unsigned
MixerGroup::mix(float *outputs, unsigned space)
{
@@ -358,44 +120,63 @@ MixerGroup::groups_required(uint32_t &groups)
}
int
MixerGroup::load_from_file(const char *path)
MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
{
if (_first != nullptr)
return -1;
int ret = -1;
const char *end = buf + buflen;
int fd = open(path, O_RDONLY);
/*
* Loop until either we have emptied the buffer, or we have failed to
* allocate something when we expected to.
*/
while (buflen > 0) {
Mixer *m = nullptr;
const char *p = end - buflen;
unsigned resid = buflen;
if (fd < 0) {
debug("failed to open %s", path);
return -1;
}
/*
* Use the next character as a hint to decide which mixer class to construct.
*/
switch (*p) {
case 'Z':
m = NullMixer::from_text(p, resid);
break;
for (unsigned count = 0;; count++) {
int result;
Mixer *mixer;
case 'M':
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
result = mixer_load(_control_cb,
_cb_handle,
fd,
mixer);
case 'R':
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid);
break;
/* error? */
if (result < 0) {
debug("error");
return -1;
default:
/* it's probably junk or whitespace, skip a byte and retry */
buflen--;
continue;
}
/* EOF or error */
if (result < 1) {
printf("[mixer] loaded %u mixers\n", count);
debug("EOF");
/*
* If we constructed something, add it to the group.
*/
if (m != nullptr) {
add_mixer(m);
/* we constructed something */
ret = 0;
/* only adjust buflen if parsing was successful */
buflen = resid;
} else {
/*
* There is data in the buffer that we expected to parse, but it didn't,
* so give up for now.
*/
break;
}
debug("loaded mixer %p", mixer);
add_mixer(mixer);
}
close(fd);
return 0;
/* nothing more in the buffer for us now */
return ret;
}
apps/systemlib/mixer/mixer_multirotor.cpp
+65 -3
View File
@@ -54,6 +54,11 @@
#include "mixer.h"
/*
* Clockwise: 1
* Counter-clockwise: -1
*/
namespace
{
@@ -148,6 +153,61 @@ MultirotorMixer::~MultirotorMixer()
{
}
MultirotorMixer *
MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
MultirotorMixer::Geometry geometry;
char geomname[8];
int s[4];
int used;
if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
debug("multirotor parse failed on '%s'", buf);
return nullptr;
}
if (used > (int)buflen) {
debug("multirotor spec used %d of %u", used, buflen);
return nullptr;
}
buflen -= used;
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorMixer::QUAD_X;
} else if (!strcmp(geomname, "6+")) {
geometry = MultirotorMixer::HEX_PLUS;
} else if (!strcmp(geomname, "6x")) {
geometry = MultirotorMixer::HEX_X;
} else if (!strcmp(geomname, "8+")) {
geometry = MultirotorMixer::OCTA_PLUS;
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
} else {
debug("unrecognised geometry '%s'", geomname);
return nullptr;
}
debug("adding multirotor mixer '%s'", geomname);
return new MultirotorMixer(
control_cb,
cb_handle,
geometry,
s[0] / 10000.0f,
s[1] / 10000.0f,
s[2] / 10000.0f,
s[3] / 10000.0f);
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
@@ -167,10 +227,12 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
/* and then stay at full control */
output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */
} else {
output_factor = max_thrust;
}
apps/systemlib/mixer/mixer_simple.cpp
+334
View File
@@ -0,0 +1,334 @@
/****************************************************************************
*
* 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_simple.cpp
*
* Simple summing mixer.
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <ctype.h>
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
mixer_simple_s *mixinfo) :
Mixer(control_cb, cb_handle),
_info(mixinfo)
{
}
SimpleMixer::~SimpleMixer()
{
if (_info != nullptr)
free(_info);
}
static const char *
findtag(const char *buf, unsigned &buflen, char tag)
{
while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':'))
return buf;
buf++;
buflen--;
}
return nullptr;
}
static void
skipline(const char *buf, unsigned &buflen)
{
const char *p;
/* if we can find a CR or NL in the buffer, skip up to it */
if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
buflen -= (p - buf);
}
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
{
int ret;
int s[5];
buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12))
return -1;
if ((ret = sscanf(buf, "O: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
debug("scaler parse failed on '%s' (got %d)", buf, ret);
return -1;
}
skipline(buf, buflen);
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
scaler.offset = s[2] / 10000.0f;
scaler.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
int
SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
{
unsigned u[2];
int s[5];
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16))
return -1;
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) {
debug("control parse failed on '%s'", buf);
return -1;
}
skipline(buf, buflen);
control_group = u[0];
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.min_output = s[3] / 10000.0f;
scaler.max_output = s[4] / 10000.0f;
return 0;
}
SimpleMixer *
SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
{
SimpleMixer *sm = nullptr;
mixer_simple_s *mixinfo = nullptr;
unsigned inputs;
int used;
const char *end = buf + buflen;
/* get the base info for the mixer */
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
debug("simple parse failed on '%s'", buf);
goto out;
}
buflen -= used;
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
if (mixinfo == nullptr) {
debug("could not allocate memory for mixer info");
goto out;
}
mixinfo->control_count = inputs;
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
goto out;
for (unsigned i = 0; i < inputs; i++) {
if (parse_control_scaler(end - buflen, buflen,
mixinfo->controls[i].scaler,
mixinfo->controls[i].control_group,
mixinfo->controls[i].control_index))
goto out;
}
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("loaded mixer with %d inputs", inputs);
} else {
debug("could not allocate memory for mixer");
}
out:
if (mixinfo != nullptr)
free(mixinfo);
return sm;
}
SimpleMixer *
SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
{
SimpleMixer *sm = nullptr;
mixer_simple_s *mixinfo = nullptr;
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
if (mixinfo == nullptr) {
debug("could not allocate memory for mixer info");
goto out;
}
mixinfo->control_count = 1;
/*
* Always pull from group 0, with the input value giving the channel.
*/
mixinfo->controls[0].control_group = 0;
mixinfo->controls[0].control_index = input;
/*
* Conversion uses both the input and output side of the mixer.
*
* The input side is used to slide the control value such that the min argument
* results in a value of zero.
*
* The output side is used to apply the scaling for the min/max values so that
* the resulting output is a -1.0 ... 1.0 value for the min...max range.
*/
mixinfo->controls[0].scaler.negative_scale = 1.0f;
mixinfo->controls[0].scaler.positive_scale = 1.0f;
mixinfo->controls[0].scaler.offset = -mid;
mixinfo->controls[0].scaler.min_output = -(mid - min);
mixinfo->controls[0].scaler.max_output = (max - mid);
mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
mixinfo->output_scaler.offset = 0.0f;
mixinfo->output_scaler.min_output = -1.0f;
mixinfo->output_scaler.max_output = 1.0f;
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("PWM input mixer for %d", input);
} else {
debug("could not allocate memory for PWM input mixer");
}
out:
if (mixinfo != nullptr)
free(mixinfo);
return sm;
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_info == nullptr)
return 0;
if (space < 1)
return 0;
for (unsigned i = 0; i < _info->control_count; i++) {
float input;
_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
input);
sum += scale(_info->controls[i].scaler, input);
}
*outputs = scale(_info->output_scaler, sum);
return 1;
}
void
SimpleMixer::groups_required(uint32_t &groups)
{
for (unsigned i = 0; i < _info->control_count; i++)
groups |= 1 << _info->controls[i].control_group;
}
int
SimpleMixer::check()
{
int ret;
float junk;
/* sanity that presumes that a mixer includes a control no more than once */
/* max of 32 groups due to groups_required API */
if (_info->control_count > 32)
return -2;
/* validate the output scaler */
ret = scale_check(_info->output_scaler);
if (ret != 0)
return ret;
/* validate input scalers */
for (unsigned i = 0; i < _info->control_count; i++) {
/* verify that we can fetch the control */
if (_control_cb(_cb_handle,
_info->controls[i].control_group,
_info->controls[i].control_index,
junk) != 0) {
return -3;
}
/* validate the scaler */
ret = scale_check(_info->controls[i].scaler);
if (ret != 0)
return (10 * i + ret);
}
return 0;
}
apps/uORB/objects_common.cpp
+3
View File
@@ -71,6 +71,9 @@ ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
#include "topics/battery_status.h"
ORB_DEFINE(battery_status, struct battery_status_s);
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
apps/uORB/topics/battery_status.h
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2012-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 battery_status.h
*
* Definition of the battery status uORB topic.
*/
#ifndef BATTERY_STATUS_H_
#define BATTERY_STATUS_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* Battery voltages and status
*/
struct battery_status_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float voltage_v; /**< Battery voltage in volts, filtered */
float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(battery_status);
#endif
apps/uORB/topics/sensor_combined.h
+2 -2
View File
@@ -99,8 +99,8 @@ struct sensor_combined_s {
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */
float battery_voltage_v; /**< Battery voltage in volts, filtered */
float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 */
float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
uint32_t baro_counter; /**< Number of raw baro measurements taken */
uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */
bool battery_voltage_valid; /**< True if battery voltage can be measured */
apps/uORB/topics/vehicle_gps_position.h
+2 -3
View File
@@ -65,8 +65,8 @@ struct vehicle_gps_position_s
uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
float s_variance; // XXX testing
float p_variance; // XXX testing
float s_variance; /**< speed accuracy estimate cm/s */
float p_variance; /**< position accuracy estimate cm */
uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */
float vel_n; /**< GPS ground speed in m/s */
float vel_e; /**< GPS ground speed in m/s */
@@ -84,7 +84,6 @@ struct vehicle_gps_position_s
/* flags */
float vel_ned_valid; /**< Flag to indicate if NED speed is valid */
};
/**
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -100,6 +100,7 @@ CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += drivers/led
CONFIGURED_APPS += drivers/blinkm
CONFIGURED_APPS += drivers/stm32/tone_alarm
CONFIGURED_APPS += drivers/stm32/adc
CONFIGURED_APPS += drivers/px4fmu
CONFIGURED_APPS += drivers/hil
nuttx/configs/px4fmu/nsh/defconfig
+12 -32
View File
@@ -189,9 +189,10 @@ CONFIG_STM32_TIM1=n
CONFIG_STM32_TIM8=n
CONFIG_STM32_USART1=y
CONFIG_STM32_USART6=y
CONFIG_STM32_ADC1=n
# We use our own driver, but leave this on.
CONFIG_STM32_ADC1=y
CONFIG_STM32_ADC2=n
CONFIG_STM32_ADC3=y
CONFIG_STM32_ADC3=n
CONFIG_STM32_SDIO=n
CONFIG_STM32_SPI1=y
CONFIG_STM32_SYSCFG=y
@@ -245,18 +246,18 @@ CONFIG_USART6_SERIAL_CONSOLE=n
#Mavlink messages can be bigger than 128
CONFIG_USART1_TXBUFSIZE=512
CONFIG_USART2_TXBUFSIZE=128
CONFIG_USART3_TXBUFSIZE=128
CONFIG_UART4_TXBUFSIZE=128
CONFIG_UART5_TXBUFSIZE=64
CONFIG_USART2_TXBUFSIZE=256
CONFIG_USART3_TXBUFSIZE=256
CONFIG_UART4_TXBUFSIZE=256
CONFIG_UART5_TXBUFSIZE=256
CONFIG_USART6_TXBUFSIZE=128
CONFIG_USART1_RXBUFSIZE=512
CONFIG_USART2_RXBUFSIZE=128
CONFIG_USART3_RXBUFSIZE=128
CONFIG_UART4_RXBUFSIZE=128
CONFIG_UART5_RXBUFSIZE=128
CONFIG_USART6_RXBUFSIZE=128
CONFIG_USART2_RXBUFSIZE=256
CONFIG_USART3_RXBUFSIZE=256
CONFIG_UART4_RXBUFSIZE=256
CONFIG_UART5_RXBUFSIZE=256
CONFIG_USART6_RXBUFSIZE=256
CONFIG_USART1_BAUD=57600
CONFIG_USART2_BAUD=115200
@@ -358,27 +359,6 @@ CONFIG_STM32_I2CTIMEOUS_START_STOP=700
# XXX this is bad and we want it gone
CONFIG_I2C_WRITEREAD=y
#
# ADC configuration
#
# Enable ADC driver support.
#
# CONFIG_ADC=y : Enable the generic ADC infrastructure
# CONFIG_STM32_TIM1_ADC=y : Indicate that timer 1 will be used to trigger an ADC
# CONFIG_STM32_TIM1_ADC3=y : Assign timer 1 to drive ADC3 sampling
# CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=100 : Select a sampling frequency
#
CONFIG_ADC=y
CONFIG_STM32_TIM4_ADC3=y
# select sample frequency 1^=1.5Msamples/second
# 65535^=10samples/second 16bit-timer runs at 84/128 MHz
CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=6000
# select timer channel 0=CC1,...,3=CC4
CONFIG_STM32_ADC3_TIMTRIG=3
CONFIG_ADC_DMA=y
# only 4 places usable!
CONFIG_ADC_FIFOSIZE=5
#
# General build options
#
nuttx/configs/px4io/common/ld.script
+9
View File
@@ -74,6 +74,15 @@ SECTIONS
_etext = ABSOLUTE(.);
} > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
.ARM.extab : {
*(.ARM.extab*)
} > flash
nuttx/configs/px4io/io/appconfig
+3
View File
@@ -35,3 +35,6 @@ CONFIGURED_APPS += drivers/boards/px4io
CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += px4io
# Mixer library from systemlib
CONFIGURED_APPS += systemlib/mixer
nuttx/configs/px4io/io/defconfig
+5 -2
View File
@@ -87,6 +87,8 @@ CONFIG_ARCH_LEDS=n
CONFIG_ARCH_BUTTONS=n
CONFIG_ARCH_CALIBRATION=n
CONFIG_ARCH_DMA=n
CONFIG_ARCH_MATH_H=y
CONFIG_ARMV7M_CMNVECTOR=y
#
@@ -131,6 +133,7 @@ CONFIG_STM32_BKP=n
CONFIG_STM32_PWR=n
CONFIG_STM32_DAC=n
# APB2:
# We use our own ADC driver, but leave this on for clocking purposes.
CONFIG_STM32_ADC1=y
CONFIG_STM32_ADC2=n
# TIM1 is owned by the HRT
@@ -342,8 +345,8 @@ CONFIG_DEBUG_I2C=n
CONFIG_DEBUG_INPUT=n
CONFIG_MSEC_PER_TICK=1
CONFIG_HAVE_CXX=n
CONFIG_HAVE_CXXINITIALIZE=n
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_MM_REGIONS=1
CONFIG_MM_SMALL=y
CONFIG_ARCH_LOWPUTC=y
nuttx/drivers/serial/serial.c
+40 -3
View File
@@ -660,9 +660,11 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
int ret = dev->ops->ioctl(filep, cmd, arg);
/* Append any higher level TTY flags */
if (ret == OK)
/*
* The device ioctl() handler returns -ENOTTY when it doesn't know
* how to handle the command. Check if we can handle it here.
*/
if (ret == -ENOTTY)
{
switch (cmd)
{
@@ -686,8 +688,43 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
irqrestore(state);
*(int *)arg = count;
ret = 0;
break;
}
case FIONWRITE:
{
int count;
irqstate_t state = irqsave();
/* determine the number of bytes free in the buffer */
if (dev->xmit.head < dev->xmit.tail)
{
count = dev->xmit.tail - dev->xmit.head - 1;
}
else
{
count = dev->xmit.size - (dev->xmit.head - dev->xmit.tail) - 1;
}
irqrestore(state);
*(int *)arg = count;
ret = 0;
break;
}
}
}
/* Append any higher level TTY flags */
else if (ret == OK)
{
switch (cmd)
{
#ifdef CONFIG_SERIAL_TERMIOS
case TCGETS:
{
nuttx/include/nuttx/fs/ioctl.h
+4
View File
@@ -110,6 +110,10 @@
* OUT: Bytes readable from this fd
*/
#define FIONWRITE _FIOC(0x0005) /* IN: Location to return value (int *)
* OUT: Bytes writable to this fd
*/
/* NuttX file system ioctl definitions **************************************/
#define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE)
nuttx/include/stdio.h
+1
View File
@@ -133,6 +133,7 @@ EXTERN void perror(FAR const char *s);
EXTERN int ungetc(int c, FAR FILE *stream);
EXTERN int vprintf(FAR const char *format, va_list ap);
EXTERN int vfprintf(FAR FILE *stream, const char *format, va_list ap);
EXTERN int vdprintf(FAR int fd, const char *format, va_list ap);
EXTERN int vsprintf(FAR char *buf, const char *format, va_list ap);
EXTERN int avsprintf(FAR char **ptr, const char *fmt, va_list ap);
EXTERN int vsnprintf(FAR char *buf, size_t size, const char *format, va_list ap);
nuttx/lib/lib_internal.h
+1
View File
@@ -140,6 +140,7 @@ extern int lib_vsprintf(FAR struct lib_outstream_s *obj,
/* Defined lib_rawprintf.c */
extern int lib_rawvprintf(const char *src, va_list ap);
extern int lib_rawvdprintf(int fd, const char *fmt, va_list ap);
/* Defined lib_lowprintf.c */

Some files were not shown because too many files have changed in this diff Show More