diff options
author | px4dev <px4@purgatory.org> | 2013-05-22 21:39:30 +0200 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-05-22 21:39:30 +0200 |
commit | 437d9e4180b6248d0f0a4176c875b41deceef17d (patch) | |
tree | 689af295626df673c13994f435ed221ba6f268e5 /src/modules/px4iofirmware | |
parent | e67022f874f0fa091ed7dffd617c70c0c0253b5c (diff) | |
parent | 78d29045c4da2d5ed4cea50fc2184b57dea01951 (diff) | |
download | px4-firmware-437d9e4180b6248d0f0a4176c875b41deceef17d.tar.gz px4-firmware-437d9e4180b6248d0f0a4176c875b41deceef17d.tar.bz2 px4-firmware-437d9e4180b6248d0f0a4176c875b41deceef17d.zip |
Merge branch 'fmuv2_bringup' into fmuv2_bringup_io2
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/adc.c | 10 | ||||
-rw-r--r-- | src/modules/px4iofirmware/controls.c | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 5 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 8 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 45 | ||||
-rw-r--r-- | src/modules/px4iofirmware/safety.c | 30 |
6 files changed, 49 insertions, 52 deletions
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 670b8d635..f744698be 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -135,6 +135,9 @@ adc_init(void) return 0; } +/* + return one measurement, or 0xffff on error + */ uint16_t adc_measure(unsigned channel) { @@ -154,9 +157,10 @@ adc_measure(unsigned channel) while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ - if (hrt_elapsed_time(&now) > 1000) { + if (hrt_elapsed_time(&now) > 100) { debug("adc timeout"); - break; + perf_end(adc_perf); + return 0xffff; } } @@ -165,4 +169,4 @@ adc_measure(unsigned channel) perf_end(adc_perf); return result; -}
\ No newline at end of file +} diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index dc36f6c93..3cf9ca149 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -217,9 +217,8 @@ controls_tick() { if (assigned_channels == 0) { rc_input_lost = true; } else { - /* set RC OK flag and clear RC lost alarm */ + /* set RC OK flag */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f38593d2a..1234e2eea 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -100,7 +100,6 @@ mixer_tick(void) } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; } source = MIX_FAILSAFE; @@ -174,7 +173,7 @@ mixer_tick(void) * here. */ bool should_arm = ( - /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && @@ -246,7 +245,7 @@ void mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 1a687bd2a..29107f79f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -138,7 +138,7 @@ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ +#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -168,9 +168,9 @@ #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ -#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ -#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ @@ -178,8 +178,6 @@ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */ -#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6c09def9e..9698a0f2f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -138,15 +138,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_VBATT_SCALE] = 10000, - [PX4IO_P_SETUP_IBATT_SCALE] = 0, - [PX4IO_P_SETUP_IBATT_BIAS] = 0, [PX4IO_P_SETUP_SET_DEBUG] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) -#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \ +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK) #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -204,7 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; - r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; @@ -313,7 +311,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) { + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; } @@ -364,7 +362,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { break; } @@ -414,18 +412,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { count++; } - /* assert deadzone is sane */ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - // The following check isn't needed as constraint checks in controls.c will catch this. - //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - // count++; - //} - //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - // count++; - //} if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { count++; } @@ -508,20 +498,27 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * 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; + if (counts != 0xffff) { + 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; + 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; + /* + 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_IN5); + if (counts != 0xffff) { + r_page_status[PX4IO_P_STATUS_IBATT] = counts; + } } SELECT_PAGE(r_page_status); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 5495d5ae1..4dbecc274 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -32,7 +32,8 @@ ****************************************************************************/ /** - * @file Safety button logic. + * @file safety.c + * Safety button logic. */ #include <nuttx/config.h> @@ -56,11 +57,11 @@ static unsigned counter = 0; /* * Define the various LED flash sequences for each system state. */ -#define LED_PATTERN_SAFE 0xffff /**< always on */ -#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ -#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ -#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ -#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ +#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ +#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ +#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ +#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ static unsigned blink_counter = 0; @@ -109,7 +110,8 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -120,8 +122,6 @@ safety_check_button(void *arg) counter++; } - /* Disarm quickly */ - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -138,21 +138,21 @@ safety_check_button(void *arg) } /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ - uint16_t pattern = LED_PATTERN_SAFE; + uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { - if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; } else { pattern = LED_PATTERN_IO_ARMED; } - } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { + pattern = LED_PATTERN_FMU_OK_TO_ARM; - } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) { - pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } /* Turn the LED on if we have a 1 at the current bit position */ |