static int pcf8591_read_channel(struct device *dev, int channel) { u8 value; struct i2c_client *client = to_i2c_client(dev); struct pcf8591_data *data = i2c_get_clientdata(client); mutex_lock(&data->update_lock); if ((data->control & PCF8591_CONTROL_AICH_MASK) != channel) { data->control = (data->control & ~PCF8591_CONTROL_AICH_MASK) | channel; i2c_smbus_write_byte(client, data->control); i2c_smbus_read_byte(client); } value = i2c_smbus_read_byte(client); mutex_unlock(&data->update_lock); if ((channel == 2 && input_mode == 2) || (channel != 3 && (input_mode == 1 || input_mode == 3))) return 10 * REG_TO_SIGNED(value); else return 10 * value; }
static int pcf8591_read_channel(struct device *dev, int channel) { u8 value; struct i2c_client *client = to_i2c_client(dev); struct pcf8591_data *data = i2c_get_clientdata(client); mutex_lock(&data->update_lock); if ((data->control & PCF8591_CONTROL_AICH_MASK) != channel) { data->control = (data->control & ~PCF8591_CONTROL_AICH_MASK) | channel; i2c_smbus_write_byte(client, data->control); /* The first byte transmitted contains the conversion code of the previous read cycle. FLUSH IT! */ i2c_smbus_read_byte(client); } value = i2c_smbus_read_byte(client); mutex_unlock(&data->update_lock); if ((channel == 2 && input_mode == 2) || (channel != 3 && (input_mode == 1 || input_mode == 3))) return (10 * REG_TO_SIGNED(value)); else return (10 * value); }
void controls_tick() { /* * Gather R/C control inputs from supported sources. * * Note that if you're silly enough to connect more than * one control input source, they're going to fight each * 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); 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_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); /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * 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, &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); /* 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 * been lost. */ bool rc_input_lost = false; /* * If we received a new frame from any of the RC sources, process it. */ if (dsm_updated || sbus_updated || ppm_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 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); } } } /* 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; } /* 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); } } /* * Export the valid channel bitmap */ r_rc_valid = assigned_channels; } /* * 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_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); } /* * Handle losing RC input */ /* 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) { /* Set channel count to zero */ r_raw_rc_count = 0; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we * must have R/C input. * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { bool override = false; /* * Check mapped channel 5 (can be any remote channel, * depends on RC_MAP_OVER parameter); * If the value is 'high' then the pilot has * requested override. * */ 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) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ if (dsm_updated || sbus_updated || ppm_updated) mixer_tick(); } else {
int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) { #define SELECT_PAGE(_page_name) \ do { \ *values = &_page_name[0]; \ *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ } while(0) switch (page) { /* * Handle pages that are updated dynamically at read time. */ case PX4IO_PAGE_STATUS: /* PX4IO_P_STATUS_FREEMEM */ { struct mallinfo minfo = mallinfo(); r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; } /* XXX PX4IO_P_STATUS_CPULOAD */ /* PX4IO_P_STATUS_FLAGS maintained externally */ /* PX4IO_P_STATUS_ALARMS maintained externally */ /* PX4IO_P_STATUS_VBATT */ { /* * Coefficients here derived by measurement of the 5-16V * range on one unit: * * V counts * 5 1001 * 6 1219 * 7 1436 * 8 1653 * 9 1870 * 10 2086 * 11 2303 * 12 2522 * 13 2738 * 14 2956 * 15 3172 * 16 3389 * * slope = 0.0046067 * intercept = 0.3863 * * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); unsigned mV = (4150 + (counts * 46)) / 10 - 200; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } /* PX4IO_P_STATUS_IBATT */ { unsigned counts = adc_measure(ADC_VBATT); unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000; int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]); if (corrected < 0) corrected = 0; r_page_status[PX4IO_P_STATUS_IBATT] = corrected; } SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: r_page_adc[0] = adc_measure(ADC_VBATT); r_page_adc[1] = adc_measure(ADC_IN5); SELECT_PAGE(r_page_adc); break; /* * Pages that are just a straight read of the register state. */ /* status pages */ case PX4IO_PAGE_CONFIG: SELECT_PAGE(r_page_config); break; case PX4IO_PAGE_ACTUATORS: SELECT_PAGE(r_page_actuators); break; case PX4IO_PAGE_SERVOS: SELECT_PAGE(r_page_servos); break; case PX4IO_PAGE_RAW_RC_INPUT: SELECT_PAGE(r_page_raw_rc_input); break; case PX4IO_PAGE_RC_INPUT: SELECT_PAGE(r_page_rc_input); break; /* readback of input pages */ case PX4IO_PAGE_SETUP: SELECT_PAGE(r_page_setup); break; case PX4IO_PAGE_CONTROLS: SELECT_PAGE(r_page_controls); break; case PX4IO_PAGE_RC_CONFIG: SELECT_PAGE(r_page_rc_input_config); break; case PX4IO_PAGE_DIRECT_PWM: SELECT_PAGE(r_page_servos); break; case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; default: return -1; } #undef SELECT_PAGE #undef COPY_PAGE last_page = page; last_offset = offset; /* if the offset is at or beyond the end of the page, we have no data */ if (offset >= *num_values) return -1; /* correct the data pointer and count for the offset */ *values += offset; *num_values -= offset; return 0; }
void controls_tick() { /* * Gather R/C control inputs from supported sources. * * Note that if you're silly enough to connect more than * one control input source, they're going to fight each * other. Don't do that. */ 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; 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) r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; perf_end(c_gather_sbus); /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * 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) r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); /* * In some cases we may have received a frame, but input has still * been lost. */ bool rc_input_lost = false; /* * If we received a new frame from any of the RC sources, process it. */ 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; } /* 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; } /* * 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 and clear RC lost alarm */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Export the valid channel bitmap */ r_rc_valid = assigned_channels; } /* * 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) { rc_input_lost = true; /* clear the input-kind flags here */ r_status_flags &= ~( 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) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; /* Mark the arrays as empty */ r_raw_rc_count = 0; r_rc_valid = 0; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we * must have R/C input. * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { bool override = false; /* * Check mapped channel 5 (can be any remote channel, * depends on RC_MAP_OVER parameter); * If the value is 'high' then the pilot has * requested override. * */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) override = true; if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ if (dsm_updated || sbus_updated || ppm_updated) mixer_tick(); } else {
int test_conv(int argc, char *argv[]) { //PX4_INFO("Testing system conversions"); for (int i = -10000; i <= 10000; i += 1) { float f = i / 10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } //PX4_INFO("All conversions clean"); return 0; }