diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f24beaf539..07c65d8cd1 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include @@ -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) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index c208db3ad2..2733877a2e 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -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 diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 98232a1a24..d656025872 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -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 { diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 3a049bb299..2632604ca0 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -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 */ }; /** diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index fef0a9ba4e..ef1584026f 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -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) */