forked from Archive/PX4-Autopilot
px4io: do not include failsafe condition into rc_lost flag
This commit is contained in:
Holger Steinhaus
Lorenz Meier
parent
cb3a4f1267
commit
409fa565f4
|
|
@ -201,94 +201,90 @@ controls_tick() {
|
||||||
/* update RC-received timestamp */
|
/* update RC-received timestamp */
|
||||||
system_state.rc_channels_timestamp_received = hrt_absolute_time();
|
system_state.rc_channels_timestamp_received = hrt_absolute_time();
|
||||||
|
|
||||||
/* do not command anything in failsafe, kick in the RC loss counter */
|
/* update RC-received timestamp */
|
||||||
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
|
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
|
||||||
|
|
||||||
/* update RC-received timestamp */
|
/* map raw inputs to mapped inputs */
|
||||||
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
|
/* XXX mapping should be atomic relative to protocol */
|
||||||
|
for (unsigned i = 0; i < r_raw_rc_count; i++) {
|
||||||
|
|
||||||
/* map raw inputs to mapped inputs */
|
/* map the input channel */
|
||||||
/* XXX mapping should be atomic relative to protocol */
|
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
|
||||||
for (unsigned i = 0; i < r_raw_rc_count; i++) {
|
|
||||||
|
|
||||||
/* map the input channel */
|
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||||
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
|
|
||||||
|
|
||||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
uint16_t raw = r_raw_rc_values[i];
|
||||||
|
|
||||||
uint16_t raw = r_raw_rc_values[i];
|
int16_t scaled;
|
||||||
|
|
||||||
int16_t scaled;
|
/*
|
||||||
|
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||||
|
*/
|
||||||
|
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||||
|
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||||
|
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||||
|
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
* 2) Scale around the mid point differently for lower and upper range.
|
||||||
*/
|
*
|
||||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
* This is necessary as they don't share the same endpoints and slope.
|
||||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
*
|
||||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
* First normalize to 0..1 range with correct sign (below or above center),
|
||||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
* then scale to 20000 range (if center is an actual center, -10000..10000,
|
||||||
|
* if parameters only support half range, scale to 10000 range, e.g. if
|
||||||
|
* center == min 0..10000, if center == max -10000..0).
|
||||||
|
*
|
||||||
|
* As the min and max bounds were enforced in step 1), division by zero
|
||||||
|
* cannot occur, as for the case of center == min or center == max the if
|
||||||
|
* statement is mutually exclusive with the arithmetic NaN case.
|
||||||
|
*
|
||||||
|
* DO NOT REMOVE OR ALTER STEP 1!
|
||||||
|
*/
|
||||||
|
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||||
|
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
||||||
|
|
||||||
/*
|
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||||
* 2) Scale around the mid point differently for lower and upper range.
|
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
||||||
*
|
|
||||||
* This is necessary as they don't share the same endpoints and slope.
|
|
||||||
*
|
|
||||||
* First normalize to 0..1 range with correct sign (below or above center),
|
|
||||||
* then scale to 20000 range (if center is an actual center, -10000..10000,
|
|
||||||
* if parameters only support half range, scale to 10000 range, e.g. if
|
|
||||||
* center == min 0..10000, if center == max -10000..0).
|
|
||||||
*
|
|
||||||
* As the min and max bounds were enforced in step 1), division by zero
|
|
||||||
* cannot occur, as for the case of center == min or center == max the if
|
|
||||||
* statement is mutually exclusive with the arithmetic NaN case.
|
|
||||||
*
|
|
||||||
* DO NOT REMOVE OR ALTER STEP 1!
|
|
||||||
*/
|
|
||||||
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
|
||||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
|
||||||
|
|
||||||
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
} else {
|
||||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
/* in the configured dead zone, output zero */
|
||||||
|
scaled = 0;
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
/* invert channel if requested */
|
||||||
/* in the configured dead zone, output zero */
|
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
||||||
scaled = 0;
|
scaled = -scaled;
|
||||||
}
|
|
||||||
|
|
||||||
/* invert channel if requested */
|
/* and update the scaled/mapped version */
|
||||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||||
|
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
||||||
|
|
||||||
|
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||||
|
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
||||||
scaled = -scaled;
|
scaled = -scaled;
|
||||||
|
|
||||||
/* and update the scaled/mapped version */
|
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
assigned_channels |= (1 << mapped);
|
||||||
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
|
||||||
|
|
||||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
|
||||||
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
|
||||||
scaled = -scaled;
|
|
||||||
|
|
||||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
|
||||||
assigned_channels |= (1 << mapped);
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/* set un-assigned controls to zero */
|
/* set un-assigned controls to zero */
|
||||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||||
if (!(assigned_channels & (1 << i)))
|
if (!(assigned_channels & (1 << i)))
|
||||||
r_rc_values[i] = 0;
|
r_rc_values[i] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* set RC OK flag, as we got an update */
|
/* set RC OK flag, as we got an update */
|
||||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
|
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
|
||||||
|
|
||||||
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
|
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
|
||||||
if (assigned_channels > 4) {
|
if (assigned_channels > 4) {
|
||||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
|
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
|
||||||
} else {
|
} else {
|
||||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
@ -316,8 +312,8 @@ controls_tick() {
|
||||||
* Handle losing RC input
|
* Handle losing RC input
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* this kicks in if the receiver is gone or the system went to failsafe */
|
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
|
||||||
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
|
if (rc_input_lost) {
|
||||||
/* Clear the RC input status flag, clear manual override flag */
|
/* Clear the RC input status flag, clear manual override flag */
|
||||||
r_status_flags &= ~(
|
r_status_flags &= ~(
|
||||||
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue