aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/px4fmu/fmu.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/px4fmu/fmu.cpp')
-rw-r--r--src/drivers/px4fmu/fmu.cpp288
1 files changed, 237 insertions, 51 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a98b527b1..6d4019f24 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -34,7 +34,7 @@
/**
* @file fmu.cpp
*
- * Driver/configurator for the PX4 FMU multi-purpose port.
+ * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
*/
#include <nuttx/config.h>
@@ -57,9 +57,10 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
-#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <drivers/drv_hrt.h>
+# include <board_config.h>
+
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
@@ -69,17 +70,21 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
+#ifdef HRT_PPM_CHANNEL
+# include <systemlib/ppm_decode.h>
+#endif
class PX4FMU : public device::CDev
{
public:
enum Mode {
+ MODE_NONE,
MODE_2PWM,
MODE_4PWM,
- MODE_NONE
+ MODE_6PWM,
};
PX4FMU();
virtual ~PX4FMU();
@@ -104,7 +109,7 @@ private:
unsigned _current_update_rate;
int _task;
int _t_actuators;
- int _t_armed;
+ int _t_actuator_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
@@ -146,6 +151,7 @@ private:
};
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
@@ -154,6 +160,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+ {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+ {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
+ {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
+ {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
+ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
+
+ {0, GPIO_VDD_5V_PERIPH_EN, 0},
+ {0, GPIO_VDD_3V3_SENSORS_EN, 0},
+ {GPIO_VDD_BRICK_VALID, 0, 0},
+ {GPIO_VDD_SERVO_VALID, 0, 0},
+ {GPIO_VDD_5V_HIPOWER_OC, 0, 0},
+ {GPIO_VDD_5V_PERIPH_OC, 0, 0},
+#endif
};
const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -174,7 +196,7 @@ PX4FMU::PX4FMU() :
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
- _t_armed(-1),
+ _t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
@@ -271,9 +293,36 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
- case MODE_2PWM: // multi-port with flow control lines as PWM
- case MODE_4PWM: // multi-port as 4 PWM outs
- debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
+ case MODE_2PWM: // v1 multi-port with flow control lines as PWM
+ debug("MODE_2PWM");
+
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0x3);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+ break;
+
+ case MODE_4PWM: // v1 multi-port as 4 PWM outs
+ debug("MODE_4PWM");
+
+ /* default output rates */
+ _pwm_default_rate = 50;
+ _pwm_alt_rate = 50;
+ _pwm_alt_rate_channels = 0;
+
+ /* XXX magic numbers */
+ up_pwm_servo_init(0xf);
+ set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+
+ break;
+
+ case MODE_6PWM: // v2 PWMs as 6 PWM outs
+ debug("MODE_6PWM");
/* default output rates */
_pwm_default_rate = 50;
@@ -281,7 +330,7 @@ PX4FMU::set_mode(Mode mode)
_pwm_alt_rate_channels = 0;
/* XXX magic numbers */
- up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
+ up_pwm_servo_init(0x3f);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
@@ -376,8 +425,8 @@ PX4FMU::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
- _t_armed = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -396,17 +445,17 @@ PX4FMU::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_armed;
+ fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
- unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
-
+#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+#endif
log("starting");
@@ -460,6 +509,23 @@ PX4FMU::task_main()
/* can we mix? */
if (_mixers != nullptr) {
+ unsigned num_outputs;
+
+ switch (_mode) {
+ case MODE_2PWM:
+ num_outputs = 2;
+ break;
+ case MODE_4PWM:
+ num_outputs = 4;
+ break;
+ case MODE_6PWM:
+ num_outputs = 6;
+ break;
+ default:
+ num_outputs = 0;
+ break;
+ }
+
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
@@ -502,7 +568,7 @@ PX4FMU::task_main()
actuator_armed_s aa;
/* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
bool set_armed = aa.armed && !aa.lockdown;
@@ -512,6 +578,7 @@ PX4FMU::task_main()
}
}
+#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
@@ -531,11 +598,13 @@ PX4FMU::task_main()
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
+#endif
+
}
::close(_t_actuators);
::close(_t_actuators_effective);
- ::close(_t_armed);
+ ::close(_t_actuator_armed);
/* make sure servos are off */
up_pwm_servo_deinit();
@@ -579,6 +648,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
+ case MODE_6PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
@@ -623,16 +693,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
- case PWM_SERVO_SET(2):
+ case PWM_SERVO_SET(5):
+ case PWM_SERVO_SET(4):
+ if (_mode < MODE_6PWM) {
+ ret = -EINVAL;
+ break;
+ }
+
+ /* FALLTHROUGH */
case PWM_SERVO_SET(3):
- if (_mode != MODE_4PWM) {
+ case PWM_SERVO_SET(2):
+ if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
- case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
+ case PWM_SERVO_SET(0):
if (arg < 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
@@ -641,16 +719,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
- case PWM_SERVO_GET(2):
+ case PWM_SERVO_GET(5):
+ case PWM_SERVO_GET(4):
+ if (_mode < MODE_6PWM) {
+ ret = -EINVAL;
+ break;
+ }
+
+ /* FALLTHROUGH */
case PWM_SERVO_GET(3):
- if (_mode != MODE_4PWM) {
+ case PWM_SERVO_GET(2):
+ if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
- case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1):
+ case PWM_SERVO_GET(0):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
@@ -658,16 +744,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
+ case PWM_SERVO_GET_RATEGROUP(4):
+ case PWM_SERVO_GET_RATEGROUP(5):
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
- if (_mode == MODE_4PWM) {
+ switch (_mode) {
+ case MODE_6PWM:
+ *(unsigned *)arg = 6;
+ break;
+ case MODE_4PWM:
*(unsigned *)arg = 4;
-
- } else {
+ break;
+ case MODE_2PWM:
*(unsigned *)arg = 2;
+ break;
+ default:
+ ret = -EINVAL;
+ break;
}
break;
@@ -743,17 +839,17 @@ ssize_t
PX4FMU::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
- uint16_t values[4];
+ uint16_t values[6];
- if (count > 4) {
- // we only have 4 PWM outputs on the FMU
- count = 4;
+ if (count > 6) {
+ // we have at most 6 outputs
+ count = 6;
}
// allow for misaligned values
- memcpy(values, buffer, count*2);
+ memcpy(values, buffer, count * 2);
- for (uint8_t i=0; i<count; i++) {
+ for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
@@ -763,19 +859,28 @@ void
PX4FMU::gpio_reset(void)
{
/*
- * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
- * to input mode.
+ * Setup default GPIO config - all pins as GPIOs, input if
+ * possible otherwise output if possible.
*/
- for (unsigned i = 0; i < _ngpio; i++)
- stm32_configgpio(_gpio_tab[i].input);
+ for (unsigned i = 0; i < _ngpio; i++) {
+ if (_gpio_tab[i].input != 0) {
+ stm32_configgpio(_gpio_tab[i].input);
+ } else if (_gpio_tab[i].output != 0) {
+ stm32_configgpio(_gpio_tab[i].output);
+ }
+ }
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ /* if we have a GPIO direction control, set it to zero (input) */
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
+#endif
}
void
PX4FMU::gpio_set_function(uint32_t gpios, int function)
{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/*
* GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both.
@@ -787,6 +892,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
}
+#endif
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
@@ -809,9 +915,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
}
}
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+#endif
}
void
@@ -912,14 +1020,21 @@ fmu_new_mode(PortMode new_mode)
/* nothing more to do here */
break;
- case PORT_FULL_SERIAL:
- /* set all multi-GPIOs to serial mode */
- gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
- break;
-
case PORT_FULL_PWM:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ servo_mode = PX4FMU::MODE_6PWM;
+#endif
+ break;
+
+ /* mixed modes supported on v1 board only */
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ case PORT_FULL_SERIAL:
+ /* set all multi-GPIOs to serial mode */
+ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
break;
case PORT_GPIO_AND_SERIAL:
@@ -938,6 +1053,9 @@ fmu_new_mode(PortMode new_mode)
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_2PWM;
break;
+#endif
+ default:
+ return -1;
}
/* adjust GPIO config for serial mode(s) */
@@ -979,22 +1097,84 @@ void
test(void)
{
int fd;
-
- fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+ unsigned servo_count = 0;
+ unsigned pwm_value = 1000;
+ int direction = 1;
+
+ fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
errx(1, "open fail");
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
- if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+ if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
+ err(1, "Unable to get servo count\n");
+ }
+
+ warnx("Testing %u servos", (unsigned)servo_count);
+
+ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+ if (!console)
+ err(1, "failed opening console");
+
+ warnx("Press CTRL-C or 'c' to abort.");
+
+ for (;;) {
+ /* sweep all servos between 1000..2000 */
+ servo_position_t servos[servo_count];
+ for (unsigned i = 0; i < servo_count; i++)
+ servos[i] = pwm_value;
+
+ if (direction == 1) {
+ // use ioctl interface for one direction
+ for (unsigned i=0; i < servo_count; i++) {
+ if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
+ err(1, "servo %u set failed", i);
+ }
+ }
+ } else {
+ // and use write interface for the other direction
+ int ret = write(fd, servos, sizeof(servos));
+ if (ret != (int)sizeof(servos))
+ err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
+ }
+
+ if (direction > 0) {
+ if (pwm_value < 2000) {
+ pwm_value++;
+ } else {
+ direction = -1;
+ }
+ } else {
+ if (pwm_value > 1000) {
+ pwm_value--;
+ } else {
+ direction = 1;
+ }
+ }
- if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+ /* readback servo values */
+ for (unsigned i = 0; i < servo_count; i++) {
+ servo_position_t value;
- if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+ if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+ err(1, "error reading PWM servo %d", i);
+ if (value != servos[i])
+ errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+ }
- if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
+ /* Check if user wants to quit */
+ char c;
+ if (read(console, &c, 1) == 1) {
+ if (c == 0x03 || c == 0x63) {
+ warnx("User abort\n");
+ break;
+ }
+ }
+ }
+ close(console);
close(fd);
exit(0);
@@ -1053,12 +1233,13 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
- } else if (!strcmp(verb, "mode_serial")) {
- new_mode = PORT_FULL_SERIAL;
-
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ } else if (!strcmp(verb, "mode_serial")) {
+ new_mode = PORT_FULL_SERIAL;
+
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
@@ -1067,6 +1248,7 @@ fmu_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
+#endif
}
/* was a new mode set? */
@@ -1088,6 +1270,10 @@ fmu_main(int argc, char *argv[])
fake(argc - 1, argv + 1);
fprintf(stderr, "FMU: unrecognised command, try:\n");
- fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
+ fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ fprintf(stderr, " mode_gpio, mode_pwm, test\n");
+#endif
exit(1);
}