aboutsummaryrefslogtreecommitdiff
path: root/apps/px4
diff options
context:
space:
mode:
Diffstat (limited to 'apps/px4')
-rw-r--r--apps/px4/fmu/fmu.cpp70
1 files changed, 52 insertions, 18 deletions
diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 8e468c012..57eb12f4e 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Driver/configurator for the PX4 FMU multi-purpose port.
+ * @file fmu.cpp
+ *
+ * Driver/configurator for the PX4 FMU multi-purpose port.
*/
#include <nuttx/config.h>
@@ -89,13 +91,14 @@ private:
volatile bool _task_should_exit;
bool _armed;
- MixMixer *_mixer[_max_actuators];
+ mixer_s *_mixer[_max_actuators];
static void task_main_trampoline(int argc, char *argv[]);
- void task_main();
+ void task_main();
};
-namespace {
+namespace
+{
FMUServo *g_servo;
@@ -122,6 +125,7 @@ FMUServo::~FMUServo()
_task_should_exit = true;
unsigned i = 0;
+
do {
/* wait 20ms */
usleep(20000);
@@ -147,11 +151,13 @@ FMUServo::init()
/* do regular cdev init */
ret = CDev::init();
+
if (ret != OK)
return ret;
/* start the IO interface task */
_task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr);
+
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -176,18 +182,20 @@ FMUServo::task_main()
/* XXX magic numbers */
up_pwm_servo_init(0x3);
break;
+
case MODE_4PWM:
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
break;
+
case MODE_NONE:
/* we should never get here... */
break;
}
/* subscribe to objects that we are interested in watching */
- _t_actuators = orb_subscribe(ORB_ID(actuator_controls));
+ _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(_t_actuators, 20); /* 50Hz update rate */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
@@ -217,9 +225,10 @@ FMUServo::task_main()
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
struct actuator_controls_s ac;
+ float *controls[1] = { &ac.control[0] };
/* get controls */
- orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &ac);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
@@ -228,7 +237,7 @@ FMUServo::task_main()
if (_mixer[i] != nullptr) {
/* mix controls to the actuator */
- float output = mixer_mix(_mixer[i], &ac.control[0]);
+ float output = mixer_mix(_mixer[i], &controls[0]);
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * output));
@@ -252,7 +261,7 @@ FMUServo::task_main()
::close(_t_armed);
/* make sure servos are off */
- up_pwm_servo_deinit();
+ up_pwm_servo_deinit();
/* note - someone else is responsible for restoring the GPIO config */
@@ -267,7 +276,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
int ret = OK;
int channel;
struct MixInfo *mi;
- struct MixMixer *mm, *tmm;
+ struct mixer_s *mm, *tmm;
switch (cmd) {
case PWM_SERVO_ARM:
@@ -284,15 +293,18 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
if (arg < 2100) {
channel = cmd - PWM_SERVO_SET(0);
up_pwm_servo_set(channel, arg);
+
} else {
ret = -EINVAL;
}
+
break;
case PWM_SERVO_GET(2):
@@ -301,20 +313,23 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
- channel = cmd - PWM_SERVO_SET(0);
- *(servo_position_t *)arg = up_pwm_servo_get(channel);
- break;
- }
+ channel = cmd - PWM_SERVO_SET(0);
+ *(servo_position_t *)arg = up_pwm_servo_get(channel);
+ break;
+ }
case MIXERIOCGETMIXERCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
+
} else {
*(unsigned *)arg = 2;
}
+
break;
case MIXERIOCGETMIXER(3):
@@ -323,6 +338,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case MIXERIOCGETMIXER(1):
case MIXERIOCGETMIXER(0):
@@ -340,10 +356,12 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if MixInfo claims to be big enough, copy mixer info */
if (mi->num_controls >= _mixer[channel]->control_count) {
memcpy(&mi->mixer, _mixer[channel], MIXER_SIZE(_mixer[channel]->control_count));
+
} else {
/* just update MixInfo with actual size of the mixer */
mi->mixer.control_count = _mixer[channel]->control_count;
}
+
break;
case MIXERIOCSETMIXER(3):
@@ -352,22 +370,25 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
break;
}
+
/* FALLTHROUGH */
case MIXERIOCSETMIXER(1):
case MIXERIOCSETMIXER(0):
channel = cmd - MIXERIOCSETMIXER(0);
/* get the caller-supplied mixer and check */
- mm = (struct MixMixer *)arg;
- if (mixer_check(mm, NUM_ACTUATOR_CONTROLS)) {
+ mm = (struct mixer_s *)arg;
+
+ if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) { /* only the attitude group is supported */
ret = -EINVAL;
break;
}
/* allocate local storage and copy from the caller*/
if (mm != nullptr) {
- tmm = (struct MixMixer *)malloc(MIXER_SIZE(mm->control_count));
+ tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
+
} else {
tmm = nullptr;
}
@@ -379,16 +400,19 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if there was an old mixer, free it */
if (mm != nullptr)
free(mm);
+
break;
default:
ret = -ENOTTY;
break;
}
+
return ret;
}
-namespace {
+namespace
+{
enum PortMode {
PORT_MODE_UNSET = 0,
@@ -403,7 +427,7 @@ enum PortMode {
PortMode g_port_mode;
int
-fmu_new_mode(PortMode new_mode)
+fmu_new_mode(PortMode new_mode)
{
int fd;
int ret = OK;
@@ -412,6 +436,7 @@ fmu_new_mode(PortMode new_mode)
/* get hold of the GPIO configuration descriptor */
fd = open(GPIO_DEVICE_PATH, 0);
+
if (fd < 0)
return -errno;
@@ -464,15 +489,19 @@ fmu_new_mode(PortMode new_mode)
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0)
ioctl(fd, GPIO_SET_ALT_1, gpio_bits);
+
close(fd);
/* create new PWM driver if required */
if (servo_mode != FMUServo::MODE_NONE) {
g_servo = new FMUServo(servo_mode);
+
if (g_servo == nullptr) {
ret = -ENOMEM;
+
} else {
ret = g_servo->init();
+
if (ret != OK) {
delete g_servo;
g_servo = nullptr;
@@ -499,14 +528,19 @@ fmu_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
+
} else if (!strcmp(argv[1], "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm")) {
new_mode = PORT_FULL_PWM;
+
} else if (!strcmp(argv[1], "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
+
} else if (!strcmp(argv[1], "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-14 05:41:19 +0000