diff options
author | px4dev <px4@purgatory.org> | 2013-03-10 16:34:21 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-03-12 22:22:49 -0700 |
commit | 6cf0758b24de1e0e28e1bb65fa33a0b49b1601b9 (patch) | |
tree | 0e7a44edee6ad28d4b184341c277fea6a958acd4 /apps/px4io | |
parent | 6b947a67d07eadc7dc882edf4505377085979784 (diff) | |
download | px4-firmware-6cf0758b24de1e0e28e1bb65fa33a0b49b1601b9.tar.gz px4-firmware-6cf0758b24de1e0e28e1bb65fa33a0b49b1601b9.tar.bz2 px4-firmware-6cf0758b24de1e0e28e1bb65fa33a0b49b1601b9.zip |
Changes for multi-rate PWM output; default and alternate rates. ioctl protocol, PX4IO support.
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/protocol.h | 23 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 4 | ||||
-rw-r--r-- | apps/px4io/registers.c | 81 |
3 files changed, 81 insertions, 27 deletions
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 14d221b5b..8d8b7e941 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -76,7 +76,7 @@ #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) /* static configuration page */ -#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ @@ -88,7 +88,7 @@ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ /* dynamic status page */ -#define PX4IO_PAGE_STATUS 1 +#define PX4IO_PAGE_STATUS 1 #define PX4IO_P_STATUS_FREEMEM 0 #define PX4IO_P_STATUS_CPULOAD 1 @@ -112,28 +112,33 @@ #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#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 */ /* array of post-mix actuator outputs, -10000..10000 */ -#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of PWM servo output values, microseconds */ -#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ #define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_PAGE_RC_INPUT 5 #define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ #define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ /* array of raw ADC values */ -#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ #define PX4IO_PAGE_SETUP 100 @@ -146,8 +151,8 @@ #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 */ -#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#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) */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 7b4b07c2c..202e9d9d9 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -94,8 +94,8 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] -#define r_setup_pwm_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] -#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] +#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] +#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #define r_control_values (&r_page_controls[0]) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 645c1565d..b09937300 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -48,6 +48,7 @@ #include "protocol.h" static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); +static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); /** * PAGE 0 @@ -116,11 +117,12 @@ uint16_t r_page_rc_input[] = { }; /** - * PAGE 6 + * Scratch page; used for registers that are constructed as-read. * - * Raw ADC input. + * PAGE 6 Raw ADC input. + * PAGE 7 PWM rate maps. */ -uint16_t r_page_adc[ADC_CHANNEL_COUNT]; +uint16_t r_page_scratch[32]; /** * PAGE 100 @@ -132,8 +134,8 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_FEATURES] = 0, [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, - [PX4IO_P_SETUP_PWM_LOWRATE] = 50, - [PX4IO_P_SETUP_PWM_HIGHRATE] = 200, + [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, + [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_VBATT_SCALE] = 10000, [PX4IO_P_SETUP_IBATT_SCALE] = 0, @@ -321,26 +323,23 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; - r_setup_pwm_rates = value; - /* XXX re-configure timers */ + pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); break; - case PX4IO_P_SETUP_PWM_LOWRATE: + case PX4IO_P_SETUP_PWM_DEFAULTRATE: if (value < 50) value = 50; if (value > 400) value = 400; - r_setup_pwm_lowrate = value; - /* XXX re-configure timers */ + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; - case PX4IO_P_SETUP_PWM_HIGHRATE: + case PX4IO_P_SETUP_PWM_ALTRATE: if (value < 50) value = 50; if (value > 400) value = 400; - r_setup_pwm_highrate = value; - /* XXX re-configure timers */ + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; case PX4IO_P_SETUP_RELAYS: @@ -529,10 +528,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val break; case PX4IO_PAGE_RAW_ADC_INPUT: - r_page_adc[0] = adc_measure(ADC_VBATT); - r_page_adc[1] = adc_measure(ADC_IN5); + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + r_page_scratch[0] = adc_measure(ADC_VBATT); + r_page_scratch[1] = adc_measure(ADC_IN5); - SELECT_PAGE(r_page_adc); + 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 < IO_SERVO_COUNT; i++) + r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + + SELECT_PAGE(r_page_scratch); break; /* @@ -593,3 +601,44 @@ last_offset = offset; return 0; } + +/* + * Helper function to handle changes to the PWM rate control registers. + */ +static void +pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) +{ + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + + /* get the channel mask for this rate group */ + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + /* all channels in the group must be either high or low-rate */ + uint32_t high = map & mask; + + if (pass == 0) { + /* preflight */ + if ((high != 0) && (high != mask)) { + /* not a legal map, bail with an alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + return; + } + } else { + /* set it - errors here are unexpected */ + if (high != 0) { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } else { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } + } + } + r_setup_pwm_rates = map; + r_setup_pwm_defaultrate = defaultrate; + r_setup_pwm_altrate = altrate; +} |