int main (void) {
adc_init();
pwm_init();
uart_init(0);
SysTick_Config(48000000/1000); // systick every 1 ms
// Hz /ticks per sec
while (1) {
adc_measure(&adcval);
counter++;
if(counter == 1)
adcval_prev = adcval;
else
adcval_prev = (adcval_prev + adcval)/2;
if(counter > filter_limit-1)
{
adcval_filtered = adcval_prev;
if(adcval_filtered < min_distance)
pwm_systick_control(10);
else if (adcval_filtered < 10000)
pwm_systick_control(adcval_filtered%100);
else
pwm_systick_control(100);
counter = 0;
}
}
return(0);
}
int main(void) {
di();
OSCCON = 0x78;
led_init();
//sound_init();
radio_init();
motors_init();
adc_init();
ei();
while (1) {
/* Listen for incoming radio packet */
if (radio_rx()) {
/* If a radio packet is pending
* send it immediately
*/
if (radio_len != 0) {
/* Send a packet */
unsigned char c;
LATB |= 0x01;
radio_tx_start();
radio_tx_data(radio_len);
for (c = 0; c < radio_len; c++)
radio_tx_data(radio_data[c]);
radio_tx_finish();
LATB &= ~0x01;
}
}
/* Read ADC values sequentally */
{
static char adc_ptr = 0;
const char adc_addr[6] = {4, 15, 11, 9, 30, 13};
/* Reading value */
char adc_id = adc_addr[adc_ptr];
adc_measure(adc_id);
adc_data[adc_ptr << 1] = ADRESH;
adc_data[(adc_ptr << 1) | 1] = ADRESL;
/* Incrementing address */
adc_ptr++;
if (adc_ptr >= 6)
adc_ptr = 0;
#ifdef FALSE_DEF
/* Checking battery voltage */
if (adc_id == 13) {
/* Voltage less then 3.0V */
if (ADRESH == 0 && ADRESL < 144) {
/* Power off */
LATA = 0;
LATB = 0;
LATC = 0;
di();
SLEEP();
}
}
#endif
}
}
}
// Measures an adc value puts it into averaging buffer and maintains the average
void sensor_update(uint8_t channel, circBuf_t* readings, uint16_t* sensor_value)
{
//Read in analog values
uint16_t new_value = adc_measure(channel);
//Read value to be replaced from circular buffer
short val_replaced = readings->data[readings->windex];
//Remove value to be replaced from current average
*sensor_value -= val_replaced/ROLLING_AVERAGE_LENGTH;
//add new value to average and replace old value in buffer
*sensor_value += new_value/ROLLING_AVERAGE_LENGTH;
writeCircBuf(readings, new_value);
}
/*
* "ioctl()"-based interface to the ADC driver available to user-space programs
*/
static long adc_ioctl(
struct file *file, unsigned int cmd, unsigned long arg)
{
long rv = 0;
struct kinetis_adc_request adc_req;
switch (cmd) {
case IOCTL_KINETISADC_ENABLE_ADC:
/* Enable an ADC module */
rv = adc_enable_module(arg, 1);
break;
case IOCTL_KINETISADC_DISABLE_ADC:
/* Disable an ADC module */
rv = adc_enable_module(arg, 0);
break;
case IOCTL_KINETISADC_MEASURE:
/* Copy the request info from user-space */
rv = copy_from_user(&adc_req, (const void __user *)arg,
sizeof(struct kinetis_adc_request));
if (rv != 0) {
rv = -EFAULT;
break;
}
/* Make a synchronous measurement */
rv = adc_measure(&adc_req);
if (rv != 0)
break;
/* Copy the updated request info back to user-space */
rv = copy_to_user((void __user *)arg, &adc_req,
sizeof(struct kinetis_adc_request));
if (rv != 0)
rv = -EFAULT;
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
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 = (uint16_t *)&_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 */
#ifdef ADC_VBATT
/* PX4IO_P_STATUS_VBATT */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit, validated on sample points of another unit
*
* Data in Tools/tests-host/data folder.
*
* measured slope = 0.004585267878277 (int: 4585)
* nominal theoretic slope: 0.00459340659 (int: 4593)
* intercept = 0.016646394188076 (int: 16646)
* nominal theoretic intercept: 0.00 (int: 0)
*
*/
unsigned counts = adc_measure(ADC_VBATT);
if (counts != 0xffff) {
unsigned mV = (166460 + (counts * 45934)) / 10000;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
}
#endif
#ifdef ADC_IBATT
/* PX4IO_P_STATUS_IBATT */
{
/*
note that we have no idea what sort of
current sensor is attached, so we just
return the raw 12 bit ADC value and let the
FMU sort it out, with user selectable
configuration for their sensor
*/
unsigned counts = adc_measure(ADC_IBATT);
if (counts != 0xffff) {
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
}
}
#endif
#ifdef ADC_VSERVO
/* PX4IO_P_STATUS_VSERVO */
{
unsigned counts = adc_measure(ADC_VSERVO);
if (counts != 0xffff) {
// use 3:1 scaling on 3.3V ADC input
unsigned mV = counts * 9900 / 4096;
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
}
}
#endif
#ifdef ADC_RSSI
/* PX4IO_P_STATUS_VRSSI */
{
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
// use 1:1 scaling on 3.3V ADC input
unsigned mV = counts * 3300 / 4096;
r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
}
}
#endif
/* XXX PX4IO_P_STATUS_PRSSI */
SELECT_PAGE(r_page_status);
break;
case PX4IO_PAGE_RAW_ADC_INPUT:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
#ifdef ADC_VBATT
r_page_scratch[0] = adc_measure(ADC_VBATT);
#endif
#ifdef ADC_IBATT
r_page_scratch[1] = adc_measure(ADC_IBATT);
#endif
#ifdef ADC_VSERVO
r_page_scratch[0] = adc_measure(ADC_VSERVO);
#endif
#ifdef ADC_RSSI
r_page_scratch[1] = adc_measure(ADC_RSSI);
#endif
SELECT_PAGE(r_page_scratch);
break;
case PX4IO_PAGE_PWM_INFO:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
SELECT_PAGE(r_page_scratch);
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;
case PX4IO_PAGE_CONTROL_MIN_PWM:
SELECT_PAGE(r_page_servo_control_min);
break;
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
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;
}
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 adc_task(void* args)
{
adc_measure(ADC_PIN, 10, on_adc_complete);
run_after_delay(adc_task, NULL, 500);
}
void adc_application(void)
{
struct keyboard_event input_key;
char string_buf[10];
uint32_t adca0_val = 0;
uint32_t adca1_val = 0;
uint32_t adca2_val = 0;
uint32_t adca3_val = 0;
uint32_t adcb0_val = 0;
uint32_t adcb1_val = 0;
uint32_t adcb2_val = 0;
uint32_t adcb3_val = 0;
// Clear screen
gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
// Draw static strings outside the loop
gfx_mono_draw_string("ADC0", 100, 0, &sysfont);
gfx_mono_draw_string("ADC1", 100, 8, &sysfont);
gfx_mono_draw_string("ADC2", 100, 16, &sysfont);
gfx_mono_draw_string("ADC3", 100, 24, &sysfont);
gfx_mono_draw_string("ADC4", 0, 0, &sysfont);
gfx_mono_draw_string("ADC5", 0, 8, &sysfont);
// gfx_mono_draw_string("ADC6", 0, 16, &sysfont);
// gfx_mono_draw_string("ADC7", 0, 24, &sysfont);
gfx_mono_draw_string("Light", 0, 16, &sysfont);
gfx_mono_draw_string("NTC", 0, 24, &sysfont);
while (true) {
// Start an ADC conversion
/*adc_start_conversion(&ADCA, ADC_CH0);
adc_start_conversion(&ADCA, ADC_CH1);
adc_start_conversion(&ADCA, ADC_CH2);
adc_start_conversion(&ADCA, ADC_CH3);*/
lightsensor_measure();
ntc_measure();
adc_measure();
adc2_measure();
while (!adc_data_is_ready() && !adc2_data_is_ready() && !ntc_data_is_ready() && !lightsensor_data_is_ready()) {
// Wait until the conversion is complete
}
//adc_wait_for_interrupt_flag(&ADCA, ADC_CH0);
/*adc0_val = adc_get_result(&ADCA, ADC_CH0);
adc1_val = adc_get_result(&ADCA, ADC_CH1);
adc2_val = adc_get_result(&ADCA, ADC_CH2);
adc3_val = adc_get_result(&ADCA, ADC_CH3);*/
adca0_val = lightsensor_get_raw_value();
adca1_val = ntc_get_raw_value();
adca2_val = adc_get_raw_value();
adca3_val = adc2_get_raw_value();
snprintf(string_buf, sizeof(string_buf), "%4ld", adca0_val);
gfx_mono_draw_string(string_buf, 30, 16, &sysfont);
snprintf(string_buf, sizeof(string_buf), "%4ld", adca1_val);
gfx_mono_draw_string(string_buf, 30, 24, &sysfont);
snprintf(string_buf, sizeof(string_buf), "%4ld", adca2_val);
gfx_mono_draw_string(string_buf, 30, 0, &sysfont);
snprintf(string_buf, sizeof(string_buf), "%4ld", adca3_val);
gfx_mono_draw_string(string_buf, 30, 8, &sysfont);
// Start an ADCB conversion
/*adc_start_conversion(&ADCB, ADC_CH0);
adc_start_conversion(&ADCB, ADC_CH1);
adc_start_conversion(&ADCB, ADC_CH2);
adc_start_conversion(&ADCB, ADC_CH3);*/
adcb0_measure();
adcb1_measure();
adcb2_measure();
adcb3_measure();
while (!adcb0_data_is_ready() && !adcb1_data_is_ready() && !adcb2_data_is_ready() && !adcb3_data_is_ready()) {
// Wait until the conversion is complete
}
//adc_wait_for_interrupt_flag(&ADCB, ADC_CH0);
/*adc0_val = adc_get_result(&ADCA, ADC_CH0);
adc1_val = adc_get_result(&ADCA, ADC_CH1);
adc2_val = adc_get_result(&ADCA, ADC_CH2);
adc3_val = adc_get_result(&ADCA, ADC_CH3);*/
adcb0_val = adcb0_get_raw_value();
adcb1_val = adcb1_get_raw_value();
adcb2_val = adcb2_get_raw_value();
adcb3_val = adcb3_get_raw_value();
snprintf(string_buf, sizeof(string_buf), "%4ld", adcb0_val);
gfx_mono_draw_string(string_buf, 60, 0, &sysfont);
snprintf(string_buf, sizeof(string_buf), "%4ld", adcb1_val);
gfx_mono_draw_string(string_buf, 60, 8, &sysfont);
snprintf(string_buf, sizeof(string_buf), "%4ld", adcb2_val);
gfx_mono_draw_string(string_buf, 60, 16, &sysfont);
snprintf(string_buf, sizeof(string_buf), "%4ld", adcb3_val);
gfx_mono_draw_string(string_buf, 60, 24, &sysfont);
keyboard_get_key_state(&input_key);
if ((input_key.keycode == KEYBOARD_ENTER) &&
(input_key.type == KEYBOARD_RELEASE)) {
break;
}
}
}
void
comms_main(void)
{
comms_init();
struct pollfd fds;
fds.fd = fmu_fd;
fds.events = POLLIN;
debug("FMU: ready");
for (;;) {
/* wait for serial data, but no more than 10ms */
poll(&fds, 1, 10);
/*
* Pull bytes from FMU and feed them to the HX engine.
* Limit the number of bytes we actually process on any one iteration.
*/
if (fds.revents & POLLIN) {
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]);
}
/*
* Decide if it's time to send an update to the FMU.
*/
static hrt_abstime last_report_time;
hrt_abstime now, delta;
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
last_report_time = now;
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++) {
report.rc_channel[i] = system_state.rc_channel_data[i];
}
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
report.battery_mv = system_state.battery_mv;
report.adc_in = system_state.adc_in5;
report.overcurrent = system_state.overcurrent;
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
/*
* Fetch ADC values, check overcurrent flags, etc.
*/
static hrt_abstime last_status_time;
if ((now - last_status_time) > FMU_STATUS_INTERVAL) {
/*
* 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);
system_state.battery_mv = (4150 + (counts * 46)) / 10;
system_state.adc_in5 = adc_measure(ADC_IN5);
system_state.overcurrent =
(OVERCURRENT_SERVO ? (1 << 0) : 0) |
(OVERCURRENT_ACC ? (1 << 1) : 0);
last_status_time = now;
}
}
}
