diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index f3f753ebe6..07831f20cf 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -36,7 +36,7 @@ * * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a * pwm_output_values structure to the device, or by publishing to the - * output_pwm ObjDev. + * output_pwm ORB topic. * Writing a value of 0 to a channel suppresses any output for that * channel. */ @@ -60,7 +60,7 @@ __BEGIN_DECLS #define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output" /** - * Maximum number of PWM output channels in the system. + * Maximum number of PWM output channels supported by the device. */ #define PWM_OUTPUT_MAX_CHANNELS 16 @@ -82,14 +82,14 @@ struct pwm_output_values { }; /* - * ObjDev tag for PWM outputs. + * ORB tag for PWM outputs. */ ORB_DECLARE(output_pwm); /* * ioctl() definitions * - * Note that ioctls and ObjDev updates should not be mixed, as the + * Note that ioctls and ORB updates should not be mixed, as the * behaviour of the system in this case is not defined. */ #define _PWM_SERVO_BASE 0x2a00 @@ -100,20 +100,14 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) -/** set update rate in Hz */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +/** set alternate servo update rate */ +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) /** get the number of servos in *(unsigned *)arg */ #define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) -/** set debug level for servo IO */ -#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4) - -/** enable in-air restart */ -#define PWM_SERVO_INAIR_RESTART_ENABLE _IOC(_PWM_SERVO_BASE, 5) - -/** disable in-air restart */ -#define PWM_SERVO_INAIR_RESTART_DISABLE _IOC(_PWM_SERVO_BASE, 6) +/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ +#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) @@ -121,6 +115,10 @@ ORB_DECLARE(output_pwm); /** get a single specific servo value */ #define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) +/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels + * whose update rates must be the same. + */ +#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n) /* * Low-level PWM output interface. @@ -157,13 +155,32 @@ __EXPORT extern void up_pwm_servo_deinit(void); __EXPORT extern void up_pwm_servo_arm(bool armed); /** - * Set the servo update rate + * Set the servo update rate for all rate groups. * * @param rate The update rate in Hz to set. * @return OK on success, -ERANGE if an unsupported update rate is set. */ __EXPORT extern int up_pwm_servo_set_rate(unsigned rate); +/** + * Get a bitmap of output channels assigned to a given rate group. + * + * @param group The rate group to query. Rate groups are assigned contiguously + * starting from zero. + * @return A bitmap of channels assigned to the rate group, or zero if + * the group number has no channels. + */ +__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group); + +/** + * Set the update rate for a given rate group. + * + * @param group The rate group whose update rate will be changed. + * @param rate The update rate in Hz. + * @return OK if the group was adjusted, -ERANGE if an unsupported update rate is set. + */ +__EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate); + /** * Set the current output value for a channel. * diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 27c885ed7a..803e744698 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -86,6 +86,8 @@ #include "uploader.h" #include +#define PX4IO_SET_DEBUG _IOC(0xff00, 0) +#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) class PX4IO : public device::I2C { @@ -1223,14 +1225,15 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - printf("alarms 0x%04x%s%s%s%s%s%s\n", + printf("alarms 0x%04x%s%s%s%s%s%s%s\n", alarms, ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : "")); + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); printf("vbatt %u ibatt %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); @@ -1269,10 +1272,10 @@ PX4IO::print_status() ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); - printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", + printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), @@ -1320,36 +1323,39 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; - case PWM_SERVO_INAIR_RESTART_ENABLE: - /* set the 'in-air restart' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); - break; - - case PWM_SERVO_INAIR_RESTART_DISABLE: - /* unset the 'in-air restart' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); - break; - case PWM_SERVO_SET_UPDATE_RATE: - /* set the requested rate */ - if ((arg >= 50) && (arg <= 400)) { - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg); + /* set the requested alternate rate */ + if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); } else { ret = -EINVAL; } break; + case PWM_SERVO_SELECT_UPDATE_RATE: { + + /* blindly clear the PWM update alarm - might be set for some other reason */ + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { + ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + } + break; + } + case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; - case PWM_SERVO_SET_DEBUG: - /* set the debug level */ - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); - break; - - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { + /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { ret = -EINVAL; @@ -1361,7 +1367,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): { + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { unsigned channel = cmd - PWM_SERVO_GET(0); @@ -1379,6 +1385,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + + *(uint32_t *)arg = value; + break; + } + case GPIO_RESET: ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); break; @@ -1443,6 +1459,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } + case PX4IO_SET_DEBUG: + /* set the debug level */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); + break; + + case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); + } + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -1602,7 +1632,7 @@ px4io_main(int argc, char *argv[]) * We can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); } else { errx(1, "not loaded"); } @@ -1623,16 +1653,16 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) { - printf("[px4io] loaded\n"); - g_dev->print_status(); - } else { - printf("[px4io] not loaded\n"); - } - - exit(0); + if (g_dev != nullptr) { + printf("[px4io] loaded\n"); + g_dev->print_status(); + } else { + printf("[px4io] not loaded\n"); } + exit(0); + } + if (!strcmp(argv[1], "debug")) { if (argc <= 2) { printf("usage: px4io debug LEVEL\n"); @@ -1646,7 +1676,7 @@ px4io_main(int argc, char *argv[]) /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); + int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 2b8641f002..d7316e1f76 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -68,10 +68,6 @@ #include #include - -/* default rate (in Hz) of PWM updates */ -static uint32_t pwm_update_rate = 50; - #define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg)) #define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) @@ -93,6 +89,10 @@ static uint32_t pwm_update_rate = 50; #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +static void pwm_timer_init(unsigned timer); +static void pwm_timer_set_rate(unsigned timer, unsigned rate); +static void pwm_channel_init(unsigned channel); + static void pwm_timer_init(unsigned timer) { @@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer) /* configure the timer to free-run at 1MHz */ rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; - /* and update at the desired rate */ - rARR(timer) = (1000000 / pwm_update_rate) - 1; - - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; + /* default to updating at 50Hz */ + pwm_timer_set_rate(timer, 50); /* note that the timer is left disabled - arming is performed separately */ } @@ -272,19 +269,41 @@ up_pwm_servo_deinit(void) } int -up_pwm_servo_set_rate(unsigned rate) +up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) { - if ((rate < 50) || (rate > 400)) + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) + return -ERANGE; + if (rate > 10000) return -ERANGE; - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) - pwm_timer_set_rate(i, rate); - } + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + return ERROR; + + pwm_timer_set_rate(group, rate); return OK; } +int +up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + up_pwm_servo_set_rate_group_update(i, rate); +} + +uint32_t +up_pwm_servo_get_rate_group(unsigned group) +{ + unsigned channels = 0; + + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + channels |= (1 << i); + } + return channels; +} + void up_pwm_servo_arm(bool armed) { diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 14d221b5b5..8d8b7e941b 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 7b4b07c2c7..202e9d9d90 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 645c1565d7..b09937300c 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; +}