This commit is contained in:
Lorenz Meier 2013-01-06 11:47:30 +01:00
commit f788d452ea
6 changed files with 119 additions and 14 deletions

View File

@ -72,6 +72,7 @@
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
@ -225,7 +226,7 @@ PX4IO::PX4IO() :
_relays(0),
_switch_armed(false),
_send_needed(false),
_config_needed(false)
_config_needed(true)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -454,12 +455,6 @@ PX4IO::task_main()
}
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
/* send a config packet to IO if required */
if (_config_needed) {
_config_needed = false;
@ -474,6 +469,12 @@ PX4IO::task_main()
_mix_buf = nullptr;
_mix_buf_len = 0;
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
}
unlock();
@ -632,7 +633,7 @@ PX4IO::io_send()
cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
/* set desired PWM output rate */
cmd.servo_rate = _update_rate;
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
@ -647,6 +648,46 @@ PX4IO::config_send()
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
int val;
/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
param_get(param_find("RC_MAP_ROLL"), &val);
cfg.rc_map[0] = (uint8_t)val;
param_get(param_find("RC_MAP_PITCH"), &val);
cfg.rc_map[1] = (uint8_t)val;
param_get(param_find("RC_MAP_YAW"), &val);
cfg.rc_map[2] = (uint8_t)val;
param_get(param_find("RC_MAP_THROTTLE"), &val);
cfg.rc_map[3] = (uint8_t)val;
/* set the individual channel properties */
char nbuf[16];
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_MIN", i);
param_get(param_find(nbuf), &val);
cfg.rc_min[i] = (uint16_t)val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_TRIM", i);
param_get(param_find(nbuf), &val);
cfg.rc_trim[i] = (uint16_t)val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_MAX", i);
param_get(param_find(nbuf), &val);
cfg.rc_max[i] = (uint16_t)val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_REV", i);
param_get(param_find(nbuf), &val);
cfg.rc_rev[i] = (uint16_t)val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_DZ", i);
param_get(param_find(nbuf), &val);
cfg.rc_dz[i] = (uint16_t)val;
}
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret)

View File

@ -207,7 +207,19 @@ comms_handle_config(const void *buffer, size_t length)
if (length != sizeof(*cfg))
return;
/* XXX */
/* fetch the rc mappings */
for (unsigned i = 0; i < 4; i++) {
system_state.rc_map[i] = cfg->rc_map[i];
}
/* fetch the rc channel attributes */
for (unsigned i = 0; i < 4; i++) {
system_state.rc_min[i] = cfg->rc_min[i];
system_state.rc_trim[i] = cfg->rc_trim[i];
system_state.rc_max[i] = cfg->rc_max[i];
system_state.rc_rev[i] = cfg->rc_rev[i];
system_state.rc_dz[i] = cfg->rc_dz[i];
}
}
static void

View File

@ -167,8 +167,9 @@ ppm_input(void)
/* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels;
for (unsigned i = 0; i < ppm_decoded_channels; i++)
for (unsigned i = 0; i < ppm_decoded_channels; i++) {
system_state.rc_channel_data[i] = ppm_buffer[i];
}
/* copy the timestamp and clear it */
system_state.rc_channels_timestamp = ppm_last_valid_decode;

View File

@ -47,11 +47,13 @@
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <sched.h>
#include <debug.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/device.h>
#include <systemlib/mixer/mixer.h>
@ -65,6 +67,12 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3
/* current servo arm/disarm state */
bool mixer_servos_armed = false;
@ -72,6 +80,8 @@ bool mixer_servos_armed = false;
static uint16_t *control_values;
static int control_count;
static uint16_t rc_channel_data[PX4IO_CONTROL_CHANNELS];
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
@ -107,13 +117,34 @@ mixer_tick(void)
} else if (system_state.rc_channels > 0) {
/* when override is on or the fmu is not available, but RC is present */
control_count = system_state.rc_channels;
control_values = &system_state.rc_channel_data[0];
sched_lock();
/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1];
rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1];
rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1];
rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1];
/* get the remaining channels, no remapping needed */
for (unsigned i = 4; i < system_state.rc_channels; i++)
rc_channel_data[i] = system_state.rc_channel_data[i];
/* scale the control inputs */
rc_channel_data[THROTTLE] = ((rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) /
(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000 + 1000;
//lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
// system_state.rc_min[THROTTLE], system_state.rc_trim[THROTTLE], system_state.rc_max[THROTTLE], rc_channel_data[THROTTLE]);
control_values = &rc_channel_data[0];
sched_unlock();
} else {
/* we have no control input (no FMU, no RC) */
// XXX builtin failsafe would activate here
control_count = 0;
}
//lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);
/* this is for multicopters, etc. where manual override does not make sense */
} else {
@ -151,8 +182,9 @@ mixer_tick(void)
/*
* If we are armed, update the servo output.
*/
if (system_state.armed && system_state.arm_ok)
if (system_state.armed && system_state.arm_ok) {
up_pwm_servo_set(i, system_state.servos[i]);
}
}
}

View File

@ -59,7 +59,7 @@ struct px4io_command {
bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */
bool arm_ok; /**< FMU allows full arming */
bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */
bool manual_override_ok; /**< if true, IO performs a direct manual override */
bool manual_override_ok; /**< if true, IO performs a direct manual override */
};
/**
@ -85,7 +85,12 @@ struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
/* XXX currently nothing here */
uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */
uint16_t rc_min[4]; /**< min value for each channel */
uint16_t rc_trim[4]; /**< trim value for each channel */
uint16_t rc_max[4]; /**< max value for each channel */
uint16_t rc_rev[4]; /**< rev value for each channel */
uint16_t rc_dz[4]; /**< dz value for each channel */
};
/**

View File

@ -72,6 +72,20 @@ struct sys_state_s {
bool arm_ok; /* FMU says OK to arm */
uint16_t servo_rate; /* output rate of servos in Hz */
/**
* Remote control input(s) channel mappings
*/
uint8_t rc_map[4];
/**
* Remote control channel attributes
*/
uint16_t rc_min[4];
uint16_t rc_trim[4];
uint16_t rc_max[4];
uint16_t rc_rev[4];
uint16_t rc_dz[4];
/**
* Data from the remote control input(s)
*/