aboutsummaryrefslogtreecommitdiff
path: root/apps/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'apps/drivers')
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_adc.c4
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_init.c42
-rw-r--r--apps/drivers/hil/hil.cpp28
-rw-r--r--apps/drivers/mpu6000/mpu6000.cpp12
-rw-r--r--apps/drivers/px4fmu/fmu.cpp136
-rw-r--r--apps/drivers/px4io/px4io.cpp150
-rw-r--r--apps/drivers/stm32/drv_pwm_servo.c51
7 files changed, 312 insertions, 111 deletions
diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c
index 987894163..b55af486d 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_adc.c
+++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c
@@ -67,10 +67,10 @@
/* Identifying number of each ADC channel: Variable Resistor. */
#ifdef CONFIG_STM32_ADC3
-static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards
+static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11, 12, 13};
/* Configurations of pins used byte each ADC channels */
-static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13};
+static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11, GPIO_ADC3_IN12, GPIO_ADC3_IN13};
#endif
/************************************************************************************
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
index 57ffb77d3..bc047aa4f 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_init.c
+++ b/apps/drivers/boards/px4fmu/px4fmu_init.c
@@ -150,9 +150,7 @@ __EXPORT int nsh_archinitialize(void)
int result;
/* configure the high-resolution time/callout interface */
-#ifdef CONFIG_HRT_TIMER
hrt_init();
-#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
@@ -160,27 +158,21 @@ __EXPORT int nsh_archinitialize(void)
#endif
/* set up the serial DMA polling */
-#ifdef SERIAL_HAVE_DMA
- {
- static struct hrt_call serial_dma_call;
- struct timespec ts;
-
- /*
- * Poll at 1ms intervals for received bytes that have not triggered
- * a DMA event.
- */
- ts.tv_sec = 0;
- ts.tv_nsec = 1000000;
-
- hrt_call_every(&serial_dma_call,
- ts_to_abstime(&ts),
- ts_to_abstime(&ts),
- (hrt_callout)stm32_serial_dma_poll,
- NULL);
- }
-#endif
-
- message("\r\n");
+ static struct hrt_call serial_dma_call;
+ struct timespec ts;
+
+ /*
+ * Poll at 1ms intervals for received bytes that have not triggered
+ * a DMA event.
+ */
+ ts.tv_sec = 0;
+ ts.tv_nsec = 1000000;
+
+ hrt_call_every(&serial_dma_call,
+ ts_to_abstime(&ts),
+ ts_to_abstime(&ts),
+ (hrt_callout)stm32_serial_dma_poll,
+ NULL);
// initial LED state
drv_led_start();
@@ -209,8 +201,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
-#if defined(CONFIG_STM32_SPI3)
- /* Get the SPI port */
+ /* Get the SPI port for the microsd slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
@@ -233,7 +224,6 @@ __EXPORT int nsh_archinitialize(void)
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-#endif /* SPI3 */
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 67b16aa42..276a642fd 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -68,11 +68,11 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
-// #include <drivers/boards/HIL/HIL_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
-#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@@ -382,7 +382,6 @@ HIL::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
- usleep(1000000);
continue;
}
@@ -396,16 +395,27 @@ HIL::task_main()
if (_mixers != nullptr) {
/* do mixing */
- _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
- /* scale for PWM output 900 - 2100us */
- outputs.output[i] = 1500 + (600 * outputs.output[i]);
-
- /* output to the servo */
- // up_pwm_servo_set(i, outputs.output[i]);
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < (unsigned)outputs.noutputs &&
+ isfinite(outputs.output[i]) &&
+ outputs.output[i] >= -1.0f &&
+ outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ outputs.output[i] = 900;
+ }
}
/* and publish for anyone that cares to see */
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index ed79440cc..55b7cfa85 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -446,8 +446,12 @@ MPU6000::init()
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
- /* do CDev init for the gyro device node */
- ret = _gyro->init();
+ /* do CDev init for the gyro device node, keep it optional */
+ int gyro_ret = _gyro->init();
+
+ if (gyro_ret != OK) {
+ _gyro_topic = -1;
+ }
return ret;
}
@@ -938,7 +942,9 @@ MPU6000::measure()
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
- orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
+ if (_gyro_topic != -1) {
+ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
+ }
/* stop measuring */
perf_end(_sample_perf);
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 61dd418f8..a672bd2fb 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -58,12 +58,15 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@@ -96,6 +99,7 @@ private:
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
+ orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@@ -110,9 +114,9 @@ private:
void task_main() __attribute__((noreturn));
static int control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input);
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@@ -161,6 +165,7 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
+ _t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@@ -177,6 +182,7 @@ PX4FMU::~PX4FMU()
_task_should_exit = true;
unsigned i = 10;
+
do {
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
@@ -212,6 +218,7 @@ PX4FMU::init()
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
@@ -245,7 +252,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[])
int
PX4FMU::set_mode(Mode mode)
{
- /*
+ /*
* Configure for PWM output.
*
* Note that regardless of the configured mode, the task is always
@@ -279,6 +286,7 @@ PX4FMU::set_mode(Mode mode)
default:
return -EINVAL;
}
+
_mode = mode;
return OK;
}
@@ -315,6 +323,13 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
+ /* advertise the effective control inputs */
+ actuator_controls_effective_s controls_effective;
+ memset(&controls_effective, 0, sizeof(controls_effective));
+ /* advertise the effective control inputs */
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+ &controls_effective);
+
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@@ -331,8 +346,18 @@ PX4FMU::task_main()
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
- if (update_rate_in_ms < 2)
+
+ /* reject faster than 500 Hz updates */
+ if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
+ _update_rate = 500;
+ }
+ /* reject slower than 50 Hz updates */
+ if (update_rate_in_ms > 20) {
+ update_rate_in_ms = 20;
+ _update_rate = 50;
+ }
+
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
@@ -358,20 +383,39 @@ PX4FMU::task_main()
if (_mixers != nullptr) {
/* do mixing */
- _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.timestamp = hrt_absolute_time();
+
+ // XXX output actual limited values
+ memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
- /* scale for PWM output 900 - 2100us */
- outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < outputs.noutputs &&
+ isfinite(outputs.output[i]) &&
+ outputs.output[i] >= -1.0f &&
+ outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ outputs.output[i] = 900;
+ }
/* output to the servo */
up_pwm_servo_set(i, outputs.output[i]);
}
/* and publish for anyone that cares to see */
- orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
@@ -388,6 +432,7 @@ PX4FMU::task_main()
}
::close(_t_actuators);
+ ::close(_t_actuators_effective);
::close(_t_armed);
/* make sure servos are off */
@@ -404,9 +449,9 @@ PX4FMU::task_main()
int
PX4FMU::control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input)
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
@@ -424,15 +469,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
/* try it as a GPIO ioctl first */
ret = gpio_ioctl(filp, cmd, arg);
+
if (ret != -ENOTTY)
return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
- switch(_mode) {
+ switch (_mode) {
case MODE_2PWM:
case MODE_4PWM:
ret = pwm_ioctl(filp, cmd, arg);
break;
+
default:
debug("not in a PWM mode");
break;
@@ -550,8 +597,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
if (_mixers == nullptr) {
ret = -ENOMEM;
+
} else {
debug("loading mixers from %s", path);
@@ -582,7 +631,7 @@ void
PX4FMU::gpio_reset(void)
{
/*
- * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
+ * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
* to input mode.
*/
for (unsigned i = 0; i < _ngpio; i++)
@@ -609,17 +658,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
/* configure selected GPIOs as required */
for (unsigned i = 0; i < _ngpio; i++) {
- if (gpios & (1<<i)) {
+ if (gpios & (1 << i)) {
switch (function) {
case GPIO_SET_INPUT:
stm32_configgpio(_gpio_tab[i].input);
break;
+
case GPIO_SET_OUTPUT:
stm32_configgpio(_gpio_tab[i].output);
break;
+
case GPIO_SET_ALT_1:
if (_gpio_tab[i].alt != 0)
stm32_configgpio(_gpio_tab[i].alt);
+
break;
}
}
@@ -636,7 +688,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function)
int value = (function == GPIO_SET) ? 1 : 0;
for (unsigned i = 0; i < _ngpio; i++)
- if (gpios & (1<<i))
+ if (gpios & (1 << i))
stm32_gpiowrite(_gpio_tab[i].output, value);
}
@@ -660,7 +712,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
lock();
switch (cmd) {
-
+
case GPIO_RESET:
gpio_reset();
break;
@@ -762,6 +814,7 @@ fmu_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
+
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
g_fmu->set_pwm_rate(update_rate);
@@ -800,13 +853,18 @@ test(void)
fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
- if (fd < 0) {
- puts("open fail");
- exit(1);
- }
+ if (fd < 0)
+ errx(1, "open fail");
+
+ if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
- ioctl(fd, PWM_SERVO_ARM, 0);
- ioctl(fd, PWM_SERVO_SET(0), 1000);
+ if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed");
+
+ if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed");
+
+ if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed");
+
+ if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed");
close(fd);
@@ -816,10 +874,8 @@ test(void)
void
fake(int argc, char *argv[])
{
- if (argc < 5) {
- puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
- exit(1);
- }
+ if (argc < 5)
+ errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)");
actuator_controls_s ac;
@@ -833,10 +889,18 @@ fake(int argc, char *argv[])
orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
- if (handle < 0) {
- puts("advertise failed");
- exit(1);
- }
+ if (handle < 0)
+ errx(1, "advertise failed");
+
+ actuator_armed_s aa;
+
+ aa.armed = true;
+ aa.lockdown = false;
+
+ handle = orb_advertise(ORB_ID(actuator_armed), &aa);
+
+ if (handle < 0)
+ errx(1, "advertise failed 2");
exit(0);
}
@@ -890,15 +954,17 @@ fmu_main(int argc, char *argv[])
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[i + 1]);
+
} else {
- fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+ errx(1, "missing argument for pwm update rate (-u)");
return 1;
}
+
} else {
- fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
+ errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
}
}
- }
+ }
/* was a new mode set? */
if (new_mode != PORT_MODE_UNSET) {
@@ -915,5 +981,5 @@ fmu_main(int argc, char *argv[])
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
- return -EINVAL;
+ exit(1);
}
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index d662e634f..99f573d1d 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -55,6 +55,7 @@
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
+#include <math.h>
#include <arch/board/board.h>
@@ -70,9 +71,12 @@
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
+#include <systemlib/scheduling_priorities.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rc_channels.h>
#include <px4io/protocol.h>
@@ -89,8 +93,17 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
+ /**
+ * Set the PWM via serial update rate
+ * @warning this directly affects CPU load
+ */
+ int set_pwm_rate(int hz);
+
+ bool dump_one;
+
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
+ int _update_rate; ///< serial send rate in Hz
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@@ -102,8 +115,13 @@ private:
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
+ orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
+ actuator_controls_effective_s _controls_effective; ///< effective controls
+
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
+ int _t_vstatus; ///< system / vehicle status
+ vehicle_status_s _vstatus; ///< overall system state
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
@@ -178,13 +196,17 @@ PX4IO *g_dev;
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
+ dump_one(false),
+ _update_rate(50),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_task_should_exit(false),
_connected(false),
_t_actuators(-1),
+ _t_actuators_effective(-1),
_t_armed(-1),
+ _t_vstatus(-1),
_t_outputs(-1),
_mixers(nullptr),
_primary_pwm_device(false),
@@ -238,7 +260,7 @@ PX4IO::init()
}
/* start the IO interface task */
- _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+ _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -260,6 +282,17 @@ PX4IO::init()
return OK;
}
+int
+PX4IO::set_pwm_rate(int hz)
+{
+ if (hz > 0 && hz <= 400) {
+ _update_rate = hz;
+ return OK;
+ } else {
+ return -EINVAL;
+ }
+}
+
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
@@ -270,6 +303,7 @@ void
PX4IO::task_main()
{
log("starting");
+ int update_rate_in_ms;
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@@ -307,12 +341,20 @@ PX4IO::task_main()
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
- //int update_rate_in_ms = int(1000 / _update_rate);
- orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
+ update_rate_in_ms = int(1000 / _update_rate);
+ orb_set_interval(_t_actuators, update_rate_in_ms);
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
+ orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
+
+ /* advertise the limited control inputs */
+ memset(&_controls_effective, 0, sizeof(_controls_effective));
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
+ &_controls_effective);
+
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
@@ -323,13 +365,15 @@ PX4IO::task_main()
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
/* poll descriptor */
- pollfd fds[3];
+ pollfd fds[4];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
+ fds[3].fd = _t_vstatus;
+ fds[3].events = POLLIN;
log("ready");
@@ -358,16 +402,37 @@ PX4IO::task_main()
if (fds[1].revents & POLLIN) {
/* get controls */
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+ ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
- /* XXX is this the right count? */
- _mixers->mix(&_outputs.output[0], _max_actuators);
+ _outputs.timestamp = hrt_absolute_time();
+ _outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators);
+
+ // XXX output actual limited values
+ memcpy(&_controls_effective, &_controls, sizeof(_controls_effective));
+
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective);
/* convert to PWM values */
- for (unsigned i = 0; i < _max_actuators; i++)
- _outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < _outputs.noutputs &&
+ isfinite(_outputs.output[i]) &&
+ _outputs.output[i] >= -1.0f &&
+ _outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ _outputs.output[i] = 1500 + (600 * _outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ _outputs.output[i] = 900;
+ }
+ }
/* and flag for update */
_send_needed = true;
@@ -379,6 +444,11 @@ PX4IO::task_main()
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
_send_needed = true;
}
+
+ if (fds[3].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
+ _send_needed = true;
+ }
}
/* send an update to IO if required */
@@ -482,6 +552,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
_send_needed = true;
+ /* if monitoring, dump the received info */
+ if (dump_one) {
+ dump_one = false;
+
+ printf("IO: %s armed ", rep->armed ? "" : "not");
+ for (unsigned i = 0; i < rep->channel_count; i++)
+ printf("%d: %d ", i, rep->rc_channel[i]);
+ printf("\n");
+ }
+
out:
unlock();
}
@@ -499,14 +579,22 @@ PX4IO::io_send()
cmd.servo_command[i] = _outputs.output[i];
/* publish as we send */
- orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
+ _outputs.timestamp = hrt_absolute_time();
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
+
/* update relays */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
- /* armed and not locked down */
+ /* armed and not locked down -> arming ok */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
+ /* indicate that full autonomous position control / vector flight mode is available */
+ cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
+ /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
+ cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
+ /* set desired PWM output rate */
+ cmd.servo_rate = _update_rate;
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
@@ -695,6 +783,31 @@ test(void)
exit(0);
}
+void
+monitor(void)
+{
+ unsigned cancels = 3;
+ printf("Hit <enter> three times to exit monitor mode\n");
+
+ for (;;) {
+ pollfd fds[1];
+
+ fds[0].fd = 0;
+ fds[0].events = POLLIN;
+ poll(fds, 1, 500);
+
+ if (fds[0].revents == POLLIN) {
+ int c;
+ read(0, &c, 1);
+ if (cancels-- == 0)
+ exit(0);
+ }
+
+ if (g_dev != nullptr)
+ g_dev->dump_one = true;
+ }
+}
+
}
int
@@ -716,6 +829,16 @@ px4io_main(int argc, char *argv[])
errx(1, "driver init failed");
}
+ /* look for the optional pwm update rate for the supported modes */
+ if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
+ if (argc > 2 + 1) {
+ g_dev->set_pwm_rate(atoi(argv[2 + 1]));
+ } else {
+ fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+ return 1;
+ }
+ }
+
exit(0);
}
@@ -770,8 +893,11 @@ px4io_main(int argc, char *argv[])
!strcmp(argv[1], "rx_sbus") ||
!strcmp(argv[1], "rx_ppm"))
errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
+
if (!strcmp(argv[1], "test"))
test();
+ if (!strcmp(argv[1], "monitor"))
+ monitor();
- errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
+ errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'");
}
diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c
index 50aa34d81..954b458f5 100644
--- a/apps/drivers/stm32/drv_pwm_servo.c
+++ b/apps/drivers/stm32/drv_pwm_servo.c
@@ -60,13 +60,13 @@
#include "drv_pwm_servo.h"
-#include "chip.h"
-#include "up_internal.h"
-#include "up_arch.h"
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
-#include "stm32_internal.h"
-#include "stm32_gpio.h"
-#include "stm32_tim.h"
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
/* default rate (in Hz) of PWM updates */
@@ -143,27 +143,27 @@ pwm_channel_init(unsigned channel)
/* configure the channel */
switch (pwm_channels[channel].timer_channel) {
case 1:
- rCCMR1(timer) |= (6 << 4);
+ rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
rCCR1(timer) = pwm_channels[channel].default_value;
- rCCER(timer) |= (1 << 0);
+ rCCER(timer) |= GTIM_CCER_CC1E;
break;
case 2:
- rCCMR1(timer) |= (6 << 12);
+ rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
rCCR2(timer) = pwm_channels[channel].default_value;
- rCCER(timer) |= (1 << 4);
+ rCCER(timer) |= GTIM_CCER_CC2E;
break;
case 3:
- rCCMR2(timer) |= (6 << 4);
+ rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
rCCR3(timer) = pwm_channels[channel].default_value;
- rCCER(timer) |= (1 << 8);
+ rCCER(timer) |= GTIM_CCER_CC3E;
break;
case 4:
- rCCMR2(timer) |= (6 << 12);
+ rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
rCCR4(timer) = pwm_channels[channel].default_value;
- rCCER(timer) |= (1 << 12);
+ rCCER(timer) |= GTIM_CCER_CC4E;
break;
}
}
@@ -288,17 +288,20 @@ up_pwm_servo_set_rate(unsigned rate)
void
up_pwm_servo_arm(bool armed)
{
- /*
- * XXX this is inelgant and in particular will either jam outputs at whatever level
- * they happen to be at at the time the timers stop or generate runts.
- * The right thing is almost certainly to kill auto-reload on the timers so that
- * they just stop at the end of their count for disable, and to reset/restart them
- * for enable.
- */
-
/* iterate timers and arm/disarm appropriately */
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
- if (pwm_timers[i].base != 0)
- rCR1(i) = armed ? GTIM_CR1_CEN : 0;
+ if (pwm_timers[i].base != 0) {
+ if (armed) {
+ /* force an update to preload all registers */
+ rEGR(i) = GTIM_EGR_UG;
+
+ /* arm requires the timer be enabled */
+ rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
+
+ } else {
+ /* on disarm, just stop auto-reload so we don't generate runts */
+ rCR1(i) &= ~GTIM_CR1_ARPE;
+ }
+ }
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-10 02:18:30 +0000