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

Rough in the new mixer path for PX4IO.

This commit is contained in:
px4dev 2012-12-29 12:58:41 -08:00
parent 6ede0e2f18
commit 6b9d5dac4d
5 changed files with 132 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
apps/px4io/Makefile
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

4
apps/px4io/comms.c
View File

@ -192,7 +192,6 @@ comms_handle_command(const void *buffer, size_t length)
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
@ -208,6 +207,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
default:
frame_bad++;
break;

111
apps/px4io/mixer.c → apps/px4io/mixer.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file mixer.c
* @file mixer.cpp
*
* Control channel input/output mixer and failsafe.
*/
@ -49,8 +49,11 @@
#include <fcntl.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
#include "px4io.h"
}
/*
* Count of periodic calls in which we have no FMU input.
@ -58,28 +61,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;
/*
@ -115,17 +113,30 @@ 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] * 1000.0f) + 1000;
} 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]);
}
}
@ -145,13 +156,57 @@ 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 */
/* XXX this presents some ugly problems w.r.t failsafe and R/C input scaling that have to be addressed */
control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
return 0;
}
void
mixer_handle_text(const void *buffer, size_t length)
{
static char mixer_text[256];
static unsigned mixer_text_length = 0;
px4io_mixdata *msg = (px4io_mixdata *)buffer;
if (length < sizeof(px4io_mixdata))
return;
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
/* 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';
/* process the text buffer, adding new mixers as their descriptions can be parsed */
char *end = &mixer_text[mixer_text_length];
mixer_group.load_from_buf(&mixer_text[0], mixer_text_length);
/* copy any leftover text to the base of the buffer for re-use */
if (mixer_text_length > 0)
memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length);
break;
}
}

48
apps/px4io/protocol.h
View File

@ -47,25 +47,21 @@
#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];
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
@ -75,4 +71,34 @@ struct px4io_report {
uint8_t channel_count;
};
/**
* 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 (sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME)
#pragma pack(pop)

6
apps/px4io/px4io.h
View File

@ -84,6 +84,11 @@ struct sys_state_s
*/
uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
/*
* Mixed servo outputs
*/
uint16_t servos[IO_SERVO_COUNT];
/*
* Relay controls
*/
@ -149,6 +154,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
* Mixer
*/
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
/*
* Safety switch/LED.