zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

IO Firmware: Code style fix, fix RSSI ADC lowpass

sbg
Lorenz Meier 10 years ago
parent
44cf402c6f
commit
9a16d9ebfa
1 changed files with 65 additions and 29 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 94
      src/modules/px4iofirmware/controls.c

94
src/modules/px4iofirmware/controls.c
Unescape View File

@ -63,6 +63,7 @@ static perf_counter_t c_gather_ppm;
static int _dsm_fd; static int _dsm_fd;
static uint16_t rc_value_override = 0; static uint16_t rc_value_override = 0;
static unsigned _rssi_adc_counts = 0;
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
{ {
@ -71,17 +72,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
uint8_t n_bytes = 0; uint8_t n_bytes = 0;
uint8_t *bytes; uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (*dsm_updated) { if (*dsm_updated) {
r_raw_rc_count = temp_count & 0x7fff; r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
if (temp_count & 0x8000) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
} else {
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; 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_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
} }
perf_end(c_gather_dsm); perf_end(c_gather_dsm);
/* get data from FD and attempt to parse with DSM and ST24 libs */ /* get data from FD and attempt to parse with DSM and ST24 libs */
@ -94,7 +100,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
/* set updated flag if one complete packet was parsed */ /* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX; st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
} }
if (*st24_updated) { if (*st24_updated) {
@ -121,7 +127,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
/* set updated flag if one complete packet was parsed */ /* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX; sumd_rssi = RC_INPUT_RSSI_MAX;
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
} }
if (*sumd_updated) { if (*sumd_updated) {
@ -170,7 +176,8 @@ controls_init(void)
} }
void void
controls_tick() { controls_tick()
{
/* /*
* Gather R/C control inputs from supported sources. * Gather R/C control inputs from supported sources.
@ -184,19 +191,24 @@ controls_tick() {
uint16_t rssi = 0; uint16_t rssi = 0;
#ifdef ADC_RSSI #ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI); unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) { if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */ /* low pass*/
unsigned mV = counts * 3300 / 4096; _rssi_adc_counts = (_rssi_adc_counts * 0.99f) + (counts * 0.01f);
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
unsigned mV = _rssi_adc_counts * 3300 / 4095;
/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
/* scale to 0..253 and lowpass */
rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
if (rssi > RC_INPUT_RSSI_MAX) { if (rssi > RC_INPUT_RSSI_MAX) {
rssi = RC_INPUT_RSSI_MAX; rssi = RC_INPUT_RSSI_MAX;
} }
} }
} }
#endif #endif
/* zero RSSI if signal is lost */ /* zero RSSI if signal is lost */
@ -207,21 +219,26 @@ controls_tick() {
perf_begin(c_gather_dsm); perf_begin(c_gather_dsm);
bool dsm_updated, st24_updated, sumd_updated; bool dsm_updated, st24_updated, sumd_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
if (dsm_updated) { if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
} }
if (st24_updated) { if (st24_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
} }
if (sumd_updated) { if (sumd_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
} }
perf_end(c_gather_dsm); perf_end(c_gather_dsm);
perf_begin(c_gather_sbus); perf_begin(c_gather_sbus);
bool sbus_failsafe, sbus_frame_drop; 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); 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) { if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
@ -231,17 +248,19 @@ controls_tick() {
if (sbus_frame_drop) { if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
sbus_rssi = RC_INPUT_RSSI_MAX / 2; sbus_rssi = RC_INPUT_RSSI_MAX / 2;
} else { } else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
} }
if (sbus_failsafe) { if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else { } else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
} }
/* set RSSI to an emulated value if ADC RSSI is off */ /* set RSSI to an emulated value if ADC RSSI is off */
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
rssi = sbus_rssi; rssi = sbus_rssi;
} }
@ -257,17 +276,20 @@ controls_tick() {
*/ */
perf_begin(c_gather_ppm); perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); 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) { if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; 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_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
} }
perf_end(c_gather_ppm); perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */ /* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) {
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
}
/* store RSSI */ /* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
@ -308,10 +330,13 @@ controls_tick() {
/* /*
* 1) Constrain to min/max values, as later processing depends on bounds. * 1) Constrain to min/max values, as later processing depends on bounds.
*/ */
if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
raw = conf[PX4IO_P_RC_CONFIG_MIN]; raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) }
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
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. * 2) Scale around the mid point differently for lower and upper range.
@ -330,10 +355,12 @@ controls_tick() {
* DO NOT REMOVE OR ALTER STEP 1! * DO NOT REMOVE OR ALTER STEP 1!
*/ */
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { 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])); 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 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])); 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 { } else {
/* in the configured dead zone, output zero */ /* in the configured dead zone, output zero */
@ -347,6 +374,7 @@ controls_tick() {
/* and update the scaled/mapped version */ /* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) { if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */ /* invert channel if pitch - pulling the lever down means pitching up by convention */
@ -360,6 +388,7 @@ controls_tick() {
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else { } else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
} }
@ -389,6 +418,7 @@ controls_tick() {
/* 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);
} }
@ -408,9 +438,9 @@ controls_tick() {
/* clear the input-kind flags here */ /* clear the input-kind flags here */
r_status_flags &= ~( r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS); PX4IO_P_STATUS_FLAGS_RC_SBUS);
} }
@ -427,8 +457,8 @@ controls_tick() {
if (rc_input_lost) { 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 |
PX4IO_P_STATUS_FLAGS_RC_OK); PX4IO_P_STATUS_FLAGS_RC_OK);
/* flag raw RC as lost */ /* flag raw RC as lost */
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
@ -451,9 +481,9 @@ controls_tick() {
* Override is enabled if either the hardcoded channel / value combination * Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it. * is selected, or the AP has requested it.
*/ */
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false; bool override = false;
@ -464,8 +494,9 @@ controls_tick() {
* requested override. * requested override.
* *
*/ */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
override = true; override = true;
}
/* /*
if the FMU is dead then enable override if we have a if the FMU is dead then enable override if we have a
@ -473,20 +504,23 @@ controls_tick() {
*/ */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
override = true; override = true;
}
if (override) { if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */ /* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
mixer_tick(); mixer_tick();
}
} else { } else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
} }
} else { } else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
} }
@ -512,8 +546,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
/* PPM data exists, copy it */ /* PPM data exists, copy it */
*num_values = ppm_decoded_channels; *num_values = ppm_decoded_channels;
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
if (*num_values > PX4IO_RC_INPUT_CHANNELS) {
*num_values = PX4IO_RC_INPUT_CHANNELS; *num_values = PX4IO_RC_INPUT_CHANNELS;
}
for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
values[i] = ppm_buffer[i]; values[i] = ppm_buffer[i];

Write Preview
Loading…
Cancel
Save