dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Changes for multi-rate PWM output; default and alternate rates. ioctl protocol, PX4IO support.

This commit is contained in:
px4dev 2013-03-10 16:34:21 -07:00
parent 6b947a67d0
commit 6cf0758b24
6 changed files with 212 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
apps/drivers/drv_pwm_output.h
View File

@ -36,7 +36,7 @@
* *
* Servo values can be set with the PWM_SERVO_SET ioctl, by writing a * 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 * 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 * Writing a value of 0 to a channel suppresses any output for that
* channel. * channel.
*/ */
@ -60,7 +60,7 @@ __BEGIN_DECLS
#define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output" #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 #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); ORB_DECLARE(output_pwm);
/* /*
* ioctl() definitions * 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. * behaviour of the system in this case is not defined.
*/ */
#define _PWM_SERVO_BASE 0x2a00 #define _PWM_SERVO_BASE 0x2a00
@ -100,20 +100,14 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */ /** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** set update rate in Hz */ /** set alternate servo update rate */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) #define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** get the number of servos in *(unsigned *)arg */ /** get the number of servos in *(unsigned *)arg */
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) #define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3)
/** set debug level for servo IO */ /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4) #define PWM_SERVO_SELECT_UPDATE_RATE _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)
/** set a single servo to a specific value */ /** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) #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 */ /** get a single specific servo value */
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) #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. * 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); __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. * @param rate The update rate in Hz to set.
* @return OK on success, -ERANGE if an unsupported update rate is set. * @return OK on success, -ERANGE if an unsupported update rate is set.
*/ */
__EXPORT extern int up_pwm_servo_set_rate(unsigned rate); __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. * Set the current output value for a channel.
* *

100
apps/drivers/px4io/px4io.cpp
View File

@ -86,6 +86,8 @@
#include "uploader.h" #include "uploader.h"
#include <debug.h> #include <debug.h>
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
class PX4IO : public device::I2C 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_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); 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,
((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), ((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", 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_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); 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_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_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_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), 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); ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
break; 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: case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested rate */ /* set the requested alternate rate */
if ((arg >= 50) && (arg <= 400)) { 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_HIGHRATE, arg); ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
} else { } else {
ret = -EINVAL; ret = -EINVAL;
} }
break; 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: case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _max_actuators; *(unsigned *)arg = _max_actuators;
break; break;
case PWM_SERVO_SET_DEBUG: case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
/* 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): {
/* TODO: we could go lower for e.g. TurboPWM */
unsigned channel = cmd - PWM_SERVO_SET(0); unsigned channel = cmd - PWM_SERVO_SET(0);
if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
ret = -EINVAL; ret = -EINVAL;
@ -1361,7 +1367,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break; 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); unsigned channel = cmd - PWM_SERVO_GET(0);
@ -1379,6 +1385,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break; 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: case GPIO_RESET:
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
break; break;
@ -1443,6 +1459,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break; 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: default:
/* not a recognized value */ /* not a recognized value */
ret = -ENOTTY; ret = -ENOTTY;
@ -1602,7 +1632,7 @@ px4io_main(int argc, char *argv[])
* We can cheat and call the driver directly, as it * We can cheat and call the driver directly, as it
* doesn't reference filp in ioctl() * 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 { } else {
errx(1, "not loaded"); errx(1, "not loaded");
} }
@ -1623,16 +1653,16 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (g_dev != nullptr) { if (g_dev != nullptr) {
printf("[px4io] loaded\n"); printf("[px4io] loaded\n");
g_dev->print_status(); g_dev->print_status();
} else { } else {
printf("[px4io] not loaded\n"); printf("[px4io] not loaded\n");
}
exit(0);
} }
exit(0);
}
if (!strcmp(argv[1], "debug")) { if (!strcmp(argv[1], "debug")) {
if (argc <= 2) { if (argc <= 2) {
printf("usage: px4io debug LEVEL\n"); 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 /* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl() * 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) { if (ret != 0) {
printf("SET_DEBUG failed - %d\n", ret); printf("SET_DEBUG failed - %d\n", ret);
exit(1); exit(1);

49
apps/drivers/stm32/drv_pwm_servo.c
View File

@ -68,10 +68,6 @@
#include <stm32_gpio.h> #include <stm32_gpio.h>
#include <stm32_tim.h> #include <stm32_tim.h>
/* 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 REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) #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 rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_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 static void
pwm_timer_init(unsigned timer) pwm_timer_init(unsigned timer)
{ {
@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer)
/* configure the timer to free-run at 1MHz */ /* configure the timer to free-run at 1MHz */
rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
/* and update at the desired rate */ /* default to updating at 50Hz */
rARR(timer) = (1000000 / pwm_update_rate) - 1; pwm_timer_set_rate(timer, 50);
/* generate an update event; reloads the counter and all registers */
rEGR(timer) = GTIM_EGR_UG;
/* note that the timer is left disabled - arming is performed separately */ /* note that the timer is left disabled - arming is performed separately */
} }
@ -272,19 +269,41 @@ up_pwm_servo_deinit(void)
} }
int 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; return -ERANGE;
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
if (pwm_timers[i].base != 0) return ERROR;
pwm_timer_set_rate(i, rate);
} pwm_timer_set_rate(group, rate);
return OK; 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 void
up_pwm_servo_arm(bool armed) up_pwm_servo_arm(bool armed)
{ {

23
apps/px4io/protocol.h
View File

@ -76,7 +76,7 @@
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
/* static configuration page */ /* 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_PROTOCOL_VERSION 0 /* magic numbers TBD */
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #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 */ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
/* dynamic status page */ /* dynamic status page */
#define PX4IO_PAGE_STATUS 1 #define PX4IO_PAGE_STATUS 1
#define PX4IO_P_STATUS_FREEMEM 0 #define PX4IO_P_STATUS_FREEMEM 0
#define PX4IO_P_STATUS_CPULOAD 1 #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_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_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_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_VBATT 4 /* battery voltage in mV */
#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ #define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
/* array of post-mix actuator outputs, -10000..10000 */ /* 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 */ /* 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 */ /* 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_COUNT 0 /* number of valid channels */
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ #define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */ /* 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_VALID 0 /* bitmask of valid controls */
#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ #define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
/* array of raw ADC values */ /* 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 */ /* setup page */
#define PX4IO_PAGE_SETUP 100 #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_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_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_DEFAULTRATE 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_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_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_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_SCALE 7 /* battery current scaling factor (float) */

4
apps/px4io/px4io.h
View File

@ -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_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] #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_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_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] #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_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#define r_control_values (&r_page_controls[0]) #define r_control_values (&r_page_controls[0])

81
apps/px4io/registers.c
View File

@ -48,6 +48,7 @@
#include "protocol.h" #include "protocol.h"
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); 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 * 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 * PAGE 100
@ -132,8 +134,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_FEATURES] = 0, [PX4IO_P_SETUP_FEATURES] = 0,
[PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_LOWRATE] = 50, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_HIGHRATE] = 200, [PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_RELAYS] = 0,
[PX4IO_P_SETUP_VBATT_SCALE] = 10000, [PX4IO_P_SETUP_VBATT_SCALE] = 10000,
[PX4IO_P_SETUP_IBATT_SCALE] = 0, [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: case PX4IO_P_SETUP_PWM_RATES:
value &= PX4IO_P_SETUP_RATES_VALID; value &= PX4IO_P_SETUP_RATES_VALID;
r_setup_pwm_rates = value; pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
/* XXX re-configure timers */
break; break;
case PX4IO_P_SETUP_PWM_LOWRATE: case PX4IO_P_SETUP_PWM_DEFAULTRATE:
if (value < 50) if (value < 50)
value = 50; value = 50;
if (value > 400) if (value > 400)
value = 400; value = 400;
r_setup_pwm_lowrate = value; pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
/* XXX re-configure timers */
break; break;
case PX4IO_P_SETUP_PWM_HIGHRATE: case PX4IO_P_SETUP_PWM_ALTRATE:
if (value < 50) if (value < 50)
value = 50; value = 50;
if (value > 400) if (value > 400)
value = 400; value = 400;
r_setup_pwm_highrate = value; pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
/* XXX re-configure timers */
break; break;
case PX4IO_P_SETUP_RELAYS: case PX4IO_P_SETUP_RELAYS:
@ -529,10 +528,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
break; break;
case PX4IO_PAGE_RAW_ADC_INPUT: case PX4IO_PAGE_RAW_ADC_INPUT:
r_page_adc[0] = adc_measure(ADC_VBATT); memset(r_page_scratch, 0, sizeof(r_page_scratch));
r_page_adc[1] = adc_measure(ADC_IN5); 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; break;
/* /*
@ -593,3 +601,44 @@ last_offset = offset;
return 0; 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;
}