diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 332d4f8826..70b289fd6d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -652,40 +652,41 @@ PX4IO::config_send() /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */ param_get(param_find("RC_MAP_ROLL"), &val); - cfg.rc_map[0] = (uint8_t)val; + cfg.rc_map[0] = val; param_get(param_find("RC_MAP_PITCH"), &val); - cfg.rc_map[1] = (uint8_t)val; + cfg.rc_map[1] = val; param_get(param_find("RC_MAP_YAW"), &val); - cfg.rc_map[2] = (uint8_t)val; + cfg.rc_map[2] = val; param_get(param_find("RC_MAP_THROTTLE"), &val); - cfg.rc_map[3] = (uint8_t)val; + cfg.rc_map[3] = val; /* set the individual channel properties */ char nbuf[16]; + float float_val; 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; + sprintf(nbuf, "RC%d_MIN", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_min[i] = float_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; + sprintf(nbuf, "RC%d_TRIM", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_trim[i] = float_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; + sprintf(nbuf, "RC%d_MAX", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_max[i] = float_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; + sprintf(nbuf, "RC%d_REV", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_rev[i] = float_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; + sprintf(nbuf, "RC%d_DZ", i + 1); + param_get(param_find(nbuf), &float_val); + cfg.rc_dz[i] = float_val; } ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 95484bc9d7..93fe93b397 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -80,7 +80,7 @@ bool mixer_servos_armed = false; static uint16_t *control_values; static int control_count; -static uint16_t rc_channel_data[PX4IO_CONTROL_CHANNELS]; +static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; static int mixer_callback(uintptr_t handle, uint8_t control_group, @@ -127,14 +127,25 @@ mixer_tick(void) 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++) + 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]); + /* scale the control inputs */ + rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / + (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; + + if (rc_channel_data[THROTTLE] > 2000) { + rc_channel_data[THROTTLE] = 2000; + } + + if (rc_channel_data[THROTTLE] < 1000) { + rc_channel_data[THROTTLE] = 1000; + } + + // lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", + // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), + // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); control_values = &rc_channel_data[0]; sched_unlock(); @@ -218,7 +229,11 @@ mixer_callback(uintptr_t handle, return -1; /* scale from current PWM units (1000-2000) to mixer input values */ - control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) { + control = ((float)control_values[control_index] - 1000.0f) / 1000.0f; + } else { + control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + } return 0; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 7aa3be02a9..90236b40c9 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -89,7 +89,7 @@ struct px4io_config { 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 */ + int8_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 65d33f34be..139bbb3217 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 */ - uint8_t rc_map[4]; + uint8_t rc_map[4]; /** * Remote control channel attributes @@ -83,7 +83,7 @@ struct sys_state_s { uint16_t rc_min[4]; uint16_t rc_trim[4]; uint16_t rc_max[4]; - uint16_t rc_rev[4]; + int16_t rc_rev[4]; uint16_t rc_dz[4]; /**