aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware/controls.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware/controls.c')
-rw-r--r--src/modules/px4iofirmware/controls.c288
1 files changed, 186 insertions, 102 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3cf9ca149..941500f0d 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -50,7 +50,7 @@
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
-static bool ppm_input(uint16_t *values, uint16_t *num_values);
+static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@@ -59,14 +59,19 @@ static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
- /* DSM input */
+ /* no channels */
+ r_raw_rc_count = 0;
+ system_state.rc_channels_timestamp_received = 0;
+ system_state.rc_channels_timestamp_valid = 0;
+
+ /* DSM input (USART1) */
dsm_init("/dev/ttyS0");
- /* S.bus input */
+ /* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
- for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
@@ -94,16 +99,67 @@ controls_tick() {
* other. Don't do that.
*/
+ /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
+ uint16_t rssi = 0;
+
+#ifdef ADC_RSSI
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
+ unsigned counts = adc_measure(ADC_RSSI);
+ if (counts != 0xffff) {
+ /* use 1:1 scaling on 3.3V ADC input */
+ unsigned mV = counts * 3300 / 4096;
+
+ /* scale to 0..253 */
+ rssi = mV / 13;
+ }
+ }
+#endif
+
perf_begin(c_gather_dsm);
- bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
- if (dsm_updated)
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+ uint16_t temp_count = r_raw_rc_count;
+ bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
+ if (dsm_updated) {
+ r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+ r_raw_rc_count = temp_count & 0x7fff;
+ if (temp_count & 0x8000)
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+ else
+ r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+ }
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
- bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
- if (sbus_updated)
+
+ bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
+ bool sbus_failsafe, sbus_frame_drop;
+ bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
+ if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+
+ rssi = 255;
+
+ if (sbus_frame_drop) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+ rssi = 100;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ }
+
+ if (sbus_failsafe) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+ rssi = 0;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ }
+
+ }
+
perf_end(c_gather_sbus);
/*
@@ -112,12 +168,21 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
- bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
- if (ppm_updated)
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
+ if (ppm_updated) {
+
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ }
perf_end(c_gather_ppm);
- ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+ /* limit number of channels to allowable data size */
+ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+ r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+
+ /* store RSSI */
+ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
/*
* In some cases we may have received a frame, but input has still
@@ -130,95 +195,100 @@ controls_tick() {
*/
if (dsm_updated || sbus_updated || ppm_updated) {
- /* update RC-received timestamp */
- system_state.rc_channels_timestamp = hrt_absolute_time();
-
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
- /* map raw inputs to mapped inputs */
- /* XXX mapping should be atomic relative to protocol */
- for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
- /* map the input channel */
- 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];
-
- 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];
-
- /*
- * 2) Scale around the mid point differently for lower and upper range.
- *
- * 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])) {
- 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]));
-
- } else {
- /* in the configured dead zone, output zero */
- scaled = 0;
+ /* update RC-received timestamp */
+ system_state.rc_channels_timestamp_received = hrt_absolute_time();
+
+ /* do not command anything in failsafe, kick in the RC loss counter */
+ if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+
+ /* update RC-received timestamp */
+ system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+ /* map raw inputs to mapped inputs */
+ /* XXX mapping should be atomic relative to protocol */
+ for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+ /* map the input channel */
+ 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];
+
+ 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];
+
+ /*
+ * 2) Scale around the mid point differently for lower and upper range.
+ *
+ * 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])) {
+ 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]));
+
+ } else {
+ /* in the configured dead zone, output zero */
+ scaled = 0;
+ }
+
+ /* invert channel if requested */
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+ scaled = -scaled;
+
+ /* and update the scaled/mapped version */
+ 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;
+
+ r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+ assigned_channels |= (1 << mapped);
+
+ }
}
-
- /* invert channel if requested */
- if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
- scaled = -scaled;
-
- /* and update the scaled/mapped version */
- unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
- ASSERT(mapped < MAX_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 */
- for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
- if (!(assigned_channels & (1 << i)))
- r_rc_values[i] = 0;
- }
+ /* set un-assigned controls to zero */
+ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+ if (!(assigned_channels & (1 << i)))
+ r_rc_values[i] = 0;
+ }
- /*
- * If we got an update with zero channels, treat it as
- * a loss of input.
- *
- * This might happen if a protocol-based receiver returns an update
- * that contains no channels that we have mapped.
- */
- if (assigned_channels == 0) {
- rc_input_lost = true;
- } else {
- /* set RC OK flag */
+ /* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+
+ /* if we have enough channels (5) to control the vehicle, the mapping is ok */
+ if (assigned_channels > 4) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
+ }
}
/*
@@ -231,7 +301,7 @@ controls_tick() {
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
- if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+ if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
@@ -239,24 +309,32 @@ controls_tick() {
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
}
/*
* Handle losing RC input
*/
- if (rc_input_lost) {
+ /* this kicks in if the receiver is gone or the system went to failsafe */
+ if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
+ /* Mark all channels as invalid, as we just lost the RX */
+ r_rc_valid = 0;
+
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+ }
+
+ /* this kicks in if the receiver is completely gone */
+ if (rc_input_lost) {
- /* Mark the arrays as empty */
+ /* Set channel count to zero */
r_raw_rc_count = 0;
- r_rc_valid = 0;
}
/*
@@ -279,7 +357,7 @@ controls_tick() {
* requested override.
*
*/
- if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
override = true;
if (override) {
@@ -293,11 +371,13 @@ controls_tick() {
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
+ } else {
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
}
static bool
-ppm_input(uint16_t *values, uint16_t *num_values)
+ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
@@ -312,8 +392,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
- if (*num_values > MAX_CONTROL_CHANNELS)
- *num_values = MAX_CONTROL_CHANNELS;
+ if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+ *num_values = PX4IO_RC_INPUT_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
@@ -321,6 +401,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* clear validity */
ppm_last_valid_decode = 0;
+ /* store PPM frame length */
+ if (num_values)
+ *frame_len = ppm_frame_length;
+
/* good if we got any channels */
result = (*num_values > 0);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-11 13:22:14 +0000