forked from Archive/PX4-Autopilot
Send rc channel ordering and channel attributes from FMU to IO
This commit is contained in:
parent
1b81724ef7
commit
0a89ab7075
|
@ -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>
|
||||
|
@ -608,6 +609,35 @@ PX4IO::io_send()
|
|||
/* set desired PWM output rate */
|
||||
cmd.servo_rate = _update_rate;
|
||||
|
||||
/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
|
||||
cmd.rc_map[0] = param_find("RC_MAP_ROLL");
|
||||
cmd.rc_map[1] = param_find("RC_MAP_PITCH");
|
||||
cmd.rc_map[2] = param_find("RC_MAP_YAW");
|
||||
cmd.rc_map[3] = param_find("RC_MAP_THROTTLE");
|
||||
|
||||
/* set the individual channel properties */
|
||||
char nbuf[16];
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
sprintf(nbuf, "RC%d_MIN", i);
|
||||
cmd.rc_min[i] = (uint16_t)param_find(nbuf);
|
||||
}
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
sprintf(nbuf, "RC%d_TRIM", i);
|
||||
cmd.rc_trim[i] = (uint16_t)param_find(nbuf);
|
||||
}
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
sprintf(nbuf, "RC%d_MAX", i);
|
||||
cmd.rc_max[i] = (uint16_t)param_find(nbuf);
|
||||
}
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
sprintf(nbuf, "RC%d_REV", i);
|
||||
cmd.rc_rev[i] = (uint16_t)param_find(nbuf);
|
||||
}
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
sprintf(nbuf, "RC%d_DZ", i);
|
||||
cmd.rc_dz[i] = (uint16_t)param_find(nbuf);
|
||||
}
|
||||
|
||||
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
|
||||
|
||||
if (ret)
|
||||
|
|
|
@ -207,6 +207,19 @@ comms_handle_command(const void *buffer, size_t length)
|
|||
system_state.servo_rate = new_servo_rate;
|
||||
}
|
||||
|
||||
/* fetch the rc mappings */
|
||||
for (unsigned i = 0; i < 4; i++)
|
||||
system_state.rc_map[i] = cmd->rc_map[i];
|
||||
|
||||
/* fetch the rc channel attributes */
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
system_state.rc_min[i] = cmd->rc_min[i];
|
||||
system_state.rc_trim[i] = cmd->rc_trim[i];
|
||||
system_state.rc_max[i] = cmd->rc_max[i];
|
||||
system_state.rc_rev[i] = cmd->rc_rev[i];
|
||||
system_state.rc_dz[i] = cmd->rc_dz[i];
|
||||
}
|
||||
|
||||
/*
|
||||
* update servo values immediately.
|
||||
* the updates are done in addition also
|
||||
|
|
|
@ -67,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;
|
||||
|
||||
|
@ -112,22 +118,22 @@ mixer_tick(void)
|
|||
/* when override is on or the fmu is not available, but RC is present */
|
||||
control_count = system_state.rc_channels;
|
||||
|
||||
// Remap the channels based on the radio type.
|
||||
// FMU: Roll, Pitch, Yaw, Throttle
|
||||
// Graupner: Throttle, Roll, Pitch, Yaw
|
||||
sched_lock();
|
||||
rc_channel_data[0] = system_state.rc_channel_data[1];
|
||||
rc_channel_data[1] = system_state.rc_channel_data[2];
|
||||
rc_channel_data[2] = system_state.rc_channel_data[3];
|
||||
rc_channel_data[3] = system_state.rc_channel_data[0];
|
||||
// AETR
|
||||
//rc_channel_data[0] = system_state.rc_channel_data[0];
|
||||
//rc_channel_data[1] = system_state.rc_channel_data[1];
|
||||
//rc_channel_data[2] = system_state.rc_channel_data[3];
|
||||
//rc_channel_data[3] = system_state.rc_channel_data[2];
|
||||
for (unsigned i = 4; i < system_state.rc_channels; i++) {
|
||||
|
||||
/* 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]];
|
||||
rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH]];
|
||||
rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW]];
|
||||
rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE]];
|
||||
|
||||
/* 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;
|
||||
|
||||
control_values = &rc_channel_data[0];
|
||||
sched_unlock();
|
||||
} else {
|
||||
|
|
|
@ -59,7 +59,13 @@ 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 */
|
||||
uint16_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 */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
uint16_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)
|
||||
*/
|
||||
|
|
Loading…
Reference in New Issue