dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed RC mapping transmission, tested. Fixed RC scaling in manual mode

This commit is contained in:
Lorenz Meier 2013-01-06 14:43:37 +01:00
parent ef2efabbd4
commit 0945a2cc2f
4 changed files with 46 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
apps/drivers/px4io/px4io.cpp
View File

@ -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));

31
apps/px4io/mixer.cpp
View File

@ -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;
}

2
apps/px4io/protocol.h
View File

@ -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 */
};

4
apps/px4io/px4io.h
View File

@ -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];
/**