fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 03:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged

Browse Source
This commit is contained in:
Lorenz Meier
2013-01-06 11:47:30 +01:00
parent 4462a25ce9 7842caf3b2
commit f788d452ea
6 changed files with 119 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4io/mixer.cpp
+34 -2
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]);
}
}
}