From 1b81724ef7d9c1424b493740d887e6c3ce259f38 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 5 Jan 2013 15:46:26 +0100 Subject: Manually remap the channel assignements for testing. --- apps/px4io/mixer.cpp | 30 +++++++++++++++++++++++++++--- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index b96ff8491..98232a1a2 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -47,11 +47,13 @@ #include #include #include +#include #include #include #include +#include #include @@ -72,6 +74,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, @@ -89,7 +93,7 @@ mixer_tick(void) /* too many frames without FMU input, time to go to failsafe */ system_state.mixer_manual_override = true; system_state.mixer_fmu_available = false; - lib_lowprintf("RX timeout\n"); + //lib_lowprintf("RX timeout\n"); } /* @@ -107,13 +111,32 @@ 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]; + + // 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++) { + rc_channel_data[i] = system_state.rc_channel_data[i]; + } + 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 +174,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]); + } } } -- cgit v1.2.3 From 0a89ab7075e4d637c91e21246c4790599f046aec Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 5 Jan 2013 22:13:12 +0100 Subject: Send rc channel ordering and channel attributes from FMU to IO --- apps/drivers/px4io/px4io.cpp | 30 ++++++++++++++++++++++++++++++ apps/px4io/comms.c | 13 +++++++++++++ apps/px4io/mixer.cpp | 34 ++++++++++++++++++++-------------- apps/px4io/protocol.h | 8 +++++++- apps/px4io/px4io.h | 14 ++++++++++++++ 5 files changed, 84 insertions(+), 15 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f24beaf53..07c65d8cd 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 c208db3ad..2733877a2 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 98232a1a2..d65602587 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 3a049bb29..2632604ca 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 fef0a9ba4..ef1584026 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) */ -- cgit v1.2.3 From 7842caf3b2d5686c4e909d7d7f28758119e8918f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 6 Jan 2013 04:21:04 +0100 Subject: Moved the channel mappings and attributes to the config section --- apps/drivers/px4io/px4io.cpp | 75 +++++++++++++++++++++++++------------------- apps/px4io/comms.c | 27 ++++++++-------- apps/px4io/controls.c | 3 +- apps/px4io/mixer.cpp | 12 ++++--- apps/px4io/protocol.h | 13 ++++---- apps/px4io/px4io.h | 4 +-- 6 files changed, 74 insertions(+), 60 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 07c65d8cd..cc6a425a3 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -222,7 +222,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; @@ -447,12 +447,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; @@ -467,6 +461,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(); @@ -608,49 +608,60 @@ 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)); - /* 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"); + if (ret) + debug("send error %d", ret); +} + +void +PX4IO::config_send() +{ + px4io_config cfg; + int ret; + + 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); - cmd.rc_min[i] = (uint16_t)param_find(nbuf); + 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); - cmd.rc_trim[i] = (uint16_t)param_find(nbuf); + 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); - cmd.rc_max[i] = (uint16_t)param_find(nbuf); + 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); - cmd.rc_rev[i] = (uint16_t)param_find(nbuf); + 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); - cmd.rc_dz[i] = (uint16_t)param_find(nbuf); + param_get(param_find(nbuf), &val); + cfg.rc_dz[i] = (uint16_t)val; } - - ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); - - if (ret) - debug("send error %d", ret); -} - -void -PX4IO::config_send() -{ - px4io_config cfg; - int ret; - - cfg.f2i_config_magic = F2I_CONFIG_MAGIC; ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 2733877a2..85da65767 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -162,6 +162,20 @@ comms_handle_config(const void *buffer, size_t length) } frame_rx++; + + /* 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 @@ -207,19 +221,6 @@ 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/controls.c b/apps/px4io/controls.c index 564687b58..32fd7c33f 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -172,8 +172,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; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index d65602587..341c2e276 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -99,7 +99,7 @@ mixer_tick(void) /* too many frames without FMU input, time to go to failsafe */ system_state.mixer_manual_override = true; system_state.mixer_fmu_available = false; - //lib_lowprintf("RX timeout\n"); + lib_lowprintf("RX timeout\n"); } /* @@ -121,10 +121,10 @@ mixer_tick(void) 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]]; - 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]]; + 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++) @@ -133,6 +133,8 @@ mixer_tick(void) /* 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(); diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 2632604ca..303e4a674 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -60,12 +60,6 @@ struct px4io_command { 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 */ - 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 */ }; /** @@ -87,7 +81,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 */ }; /** diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index ef1584026..01829e520 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -75,7 +75,7 @@ struct sys_state_s { /** * Remote control input(s) channel mappings */ - uint16_t rc_map[4]; + uint8_t rc_map[4]; /** * Remote control channel attributes @@ -85,7 +85,7 @@ struct sys_state_s { uint16_t rc_max[4]; uint16_t rc_rev[4]; uint16_t rc_dz[4]; - + /** * Data from the remote control input(s) */ -- cgit v1.2.3