aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/drv_accel.h4
-rw-r--r--src/drivers/drv_airspeed.h5
-rw-r--r--src/drivers/drv_baro.h4
-rw-r--r--src/drivers/drv_gps.h4
-rw-r--r--src/drivers/drv_gyro.h4
-rw-r--r--src/drivers/drv_pwm_output.h70
-rw-r--r--src/drivers/drv_rc_input.h3
-rw-r--r--src/drivers/drv_sensor.h4
-rw-r--r--src/drivers/drv_tone_alarm.h4
-rw-r--r--src/drivers/hil/hil.cpp4
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp32
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp2
-rw-r--r--src/drivers/px4fmu/fmu.cpp369
-rw-r--r--src/drivers/px4fmu/module.mk3
-rw-r--r--src/drivers/px4io/px4io.cpp310
-rw-r--r--src/drivers/px4io/px4io_serial.cpp28
16 files changed, 526 insertions, 324 deletions
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 8a4f68428..7d93ed938 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Accelerometer driver interface.
+ * @file drv_accel.h
+ *
+ * Accelerometer driver interface.
*/
#ifndef _DRV_ACCEL_H
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index 7bb9ee2af..78f31495d 100644
--- a/src/drivers/drv_airspeed.h
+++ b/src/drivers/drv_airspeed.h
@@ -32,7 +32,10 @@
****************************************************************************/
/**
- * @file Airspeed driver interface.
+ * @file drv_airspeed.h
+ *
+ * Airspeed driver interface.
+ *
* @author Simon Wilks
*/
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index aa251889f..e2f0137ae 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Barometric pressure sensor driver interface.
+ * @file drv_baro.h
+ *
+ * Barometric pressure sensor driver interface.
*/
#ifndef _DRV_BARO_H
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 398cf4870..06e3535b3 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file GPS driver interface.
+ * @file drv_gps.h
+ *
+ * GPS driver interface.
*/
#ifndef _DRV_GPS_H
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index fefcae50b..2ae8c7058 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Gyroscope driver interface.
+ * @file drv_gyro.h
+ *
+ * Gyroscope driver interface.
*/
#ifndef _DRV_GYRO_H
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index fc96bd848..efd6afb4b 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -65,6 +65,26 @@ __BEGIN_DECLS
#define PWM_OUTPUT_MAX_CHANNELS 16
/**
+ * Minimum PWM in us
+ */
+#define PWM_MIN 900
+
+/**
+ * Highest PWM allowed as the minimum PWM
+ */
+#define PWM_HIGHEST_MIN 1300
+
+/**
+ * Maximum PWM in us
+ */
+#define PWM_MAX 2100
+
+/**
+ * Lowest PWM allowed as the maximum PWM
+ */
+#define PWM_LOWEST_MAX 1700
+
+/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.
*/
@@ -79,6 +99,7 @@ typedef uint16_t servo_position_t;
struct pwm_output_values {
/** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_CHANNELS];
+ unsigned channel_count;
};
/*
@@ -100,30 +121,63 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
+/** get default servo update rate */
+#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
/** 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, 3)
+
+/** get alternate servo update rate */
+#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** 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, 5)
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
-#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6)
+
+/** check the selected update rates */
+#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7)
/** set the 'ARM ok' bit, which activates the safety switch */
-#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
+#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
-#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
+#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9)
/** start DSM bind */
-#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
+#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10)
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
-/** Power up DSM receiver */
-#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
+/** power up DSM receiver */
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
+
+/** set the PWM value for failsafe */
+#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12)
+
+/** get the PWM value for failsafe */
+#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13)
+
+/** set the PWM value when disarmed - should be no PWM (zero) by default */
+#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14)
+
+/** get the PWM value when disarmed */
+#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15)
+
+/** set the minimum PWM value the output will send */
+#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16)
+
+/** get the minimum PWM value the output will send */
+#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17)
+
+/** set the maximum PWM value the output will send */
+#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18)
+
+/** get the maximum PWM value the output will send */
+#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 4decc324e..78ffad9d7 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -32,8 +32,9 @@
****************************************************************************/
/**
- * @file R/C input interface.
+ * @file drv_rc_input.h
*
+ * R/C input interface.
*/
#ifndef _DRV_RC_INPUT_H
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 3a89cab9d..8e8b2c1b9 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file Common sensor API and ioctl definitions.
+ * @file drv_sensor.h
+ *
+ * Common sensor API and ioctl definitions.
*/
#ifndef _DRV_SENSOR_H
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 2fab37dd2..cb5fd53a5 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -31,7 +31,9 @@
*
****************************************************************************/
-/*
+/**
+ * @file drv_tone_alarm.h
+ *
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
*
* The tone_alarm driver supports a set of predefined "alarm"
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 3e30e3292..c1d73dd87 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -404,7 +404,7 @@ HIL::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
- if (i < (unsigned)outputs.noutputs &&
+ if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
@@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
g_hil->set_pwm_rate(arg);
break;
- case PWM_SERVO_SELECT_UPDATE_RATE:
+ case PWM_SERVO_SET_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
break;
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8bed8a8df..60601e22c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -241,11 +241,18 @@ private:
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
+ perf_counter_t _reg7_resets;
+ perf_counter_t _reg1_resets;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
+ // expceted values of reg1 and reg7 to catch in-flight
+ // brownouts of the sensor
+ uint8_t _reg7_expected;
+ uint8_t _reg1_expected;
+
/**
* Start automatic measurement.
*/
@@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+ _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
+ _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
- _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
+ _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _reg1_expected(0),
+ _reg7_expected(0)
{
// enable debug() calls
_debug_enabled = true;
@@ -526,10 +537,12 @@ void
LSM303D::reset()
{
/* enable accel*/
- write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
+ write_reg(ADDR_CTRL_REG1, _reg1_expected);
/* enable mag */
- write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+ _reg7_expected = REG7_CONT_MODE_M;
+ write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
@@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency)
}
modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+ _reg1_expected = (_reg1_expected & ~clearbits) | setbits;
return OK;
}
@@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg)
void
LSM303D::measure()
{
+ if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
+ perf_count(_reg1_resets);
+ reset();
+ return;
+ }
+
/* status register and data as read back from the device */
#pragma pack(push, 1)
@@ -1282,6 +1302,12 @@ LSM303D::measure()
void
LSM303D::mag_measure()
{
+ if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
+ perf_count(_reg7_resets);
+ reset();
+ return;
+ }
+
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index d0de26a1a..b93f38cf6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK;
break;
- case PWM_SERVO_SELECT_UPDATE_RATE:
+ case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK;
break;
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b1dd55dd7..4bd27f907 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -59,11 +59,12 @@
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
-# include <board_config.h>
+#include <board_config.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
@@ -72,11 +73,17 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
-#include <systemlib/err.h>
+
#ifdef HRT_PPM_CHANNEL
# include <systemlib/ppm_decode.h>
#endif
+/*
+ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
+ */
+
+#define CONTROL_INPUT_DROP_LIMIT_MS 20
+
class PX4FMU : public device::CDev
{
public:
@@ -100,7 +107,12 @@ public:
int set_pwm_alt_channels(uint32_t channels);
private:
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
static const unsigned _max_actuators = 4;
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ static const unsigned _max_actuators = 6;
+#endif
Mode _mode;
unsigned _pwm_default_rate;
@@ -117,11 +129,20 @@ private:
volatile bool _task_should_exit;
bool _armed;
+ bool _pwm_on;
MixerGroup *_mixers;
actuator_controls_s _controls;
+ pwm_limit_t _pwm_limit;
+ uint16_t _failsafe_pwm[_max_actuators];
+ uint16_t _disarmed_pwm[_max_actuators];
+ uint16_t _min_pwm[_max_actuators];
+ uint16_t _max_pwm[_max_actuators];
+ unsigned _num_failsafe_set;
+ unsigned _num_disarmed_set;
+
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
@@ -203,8 +224,18 @@ PX4FMU::PX4FMU() :
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
- _mixers(nullptr)
+ _pwm_on(false),
+ _mixers(nullptr),
+ _failsafe_pwm({0}),
+ _disarmed_pwm({0}),
+ _num_failsafe_set(0),
+ _num_disarmed_set(0)
{
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ _min_pwm[i] = PWM_MIN;
+ _max_pwm[i] = PWM_MAX;
+ }
+
_debug_enabled = true;
}
@@ -457,6 +488,9 @@ PX4FMU::task_main()
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
+ /* initialize PWM limit lib */
+ pwm_limit_init(&_pwm_limit);
+
log("starting");
/* loop until killed */
@@ -491,90 +525,103 @@ PX4FMU::task_main()
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
- int ret = ::poll(&fds[0], 2, 10);
+ int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
- }
+ } else if (ret == 0) {
+ /* timeout: no control data, switch to failsafe values */
+// warnx("no PWM: failsafe");
- /* do we have a control update? */
- if (fds[0].revents & POLLIN) {
-
- /* get controls - must always do this to avoid spinning */
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
- /* 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;
- }
+ } else {
- /* do mixing */
- 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++) {
-
- /* 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;
+ /* do we have a control update? */
+ if (fds[0].revents & POLLIN) {
+
+ /* get controls - must always do this to avoid spinning */
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+ /* 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;
}
- /* output to the servo */
- up_pwm_servo_set(i, outputs.output[i]);
- }
+ /* do mixing */
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.timestamp = hrt_absolute_time();
+
+ /* iterate actuators */
+ for (unsigned i = 0; i < num_outputs; 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) {
+ /*
+ * 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] = -1.0f;
+ }
+ }
+
+ uint16_t pwm_limited[num_outputs];
- /* and publish for anyone that cares to see */
- orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+ pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+
+ /* output actual limited values */
+ for (unsigned i = 0; i < num_outputs; i++) {
+ controls_effective.control_effective[i] = (float)pwm_limited[i];
+ }
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+ /* output to the servos */
+ for (unsigned i = 0; i < num_outputs; i++) {
+ up_pwm_servo_set(i, pwm_limited[i]);
+ }
+
+ /* and publish for anyone that cares to see */
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+ }
}
- }
- /* how about an arming update? */
- if (fds[1].revents & POLLIN) {
- actuator_armed_s aa;
+ /* how about an arming update? */
+ if (fds[1].revents & POLLIN) {
+ actuator_armed_s aa;
+
+ /* get new value */
+ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
- /* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+ /* update the armed status and check that we're not locked down */
+ bool set_armed = aa.armed && !aa.lockdown;
+ if (_armed != set_armed)
+ _armed = set_armed;
- /* update PWM servo armed status if armed and not locked down */
- bool set_armed = aa.armed && !aa.lockdown;
- if (set_armed != _armed) {
- _armed = set_armed;
- up_pwm_servo_arm(set_armed);
+ /* update PWM status if armed or if disarmed PWM values are set */
+ bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+ if (_pwm_on != pwm_on) {
+ _pwm_on = pwm_on;
+ up_pwm_servo_arm(pwm_on);
+ }
}
}
@@ -685,14 +732,164 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
+ case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+ *(uint32_t *)arg = _pwm_default_rate;
+ break;
+
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break;
- case PWM_SERVO_SELECT_UPDATE_RATE:
+ case PWM_SERVO_GET_UPDATE_RATE:
+ *(uint32_t *)arg = _pwm_alt_rate;
+ break;
+
+ case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
+ case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+ *(uint32_t *)arg = _pwm_alt_rate_channels;
+ break;
+
+ case PWM_SERVO_SET_FAILSAFE_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] > PWM_MAX) {
+ _failsafe_pwm[i] = PWM_MAX;
+ } else if (pwm->values[i] < PWM_MIN) {
+ _failsafe_pwm[i] = PWM_MIN;
+ } else {
+ _failsafe_pwm[i] = pwm->values[i];
+ }
+ }
+
+ /*
+ * update the counter
+ * this is needed to decide if disarmed PWM output should be turned on or not
+ */
+ _num_failsafe_set = 0;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ if (_failsafe_pwm[i] > 0)
+ _num_failsafe_set++;
+ }
+ break;
+ }
+ case PWM_SERVO_GET_FAILSAFE_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _failsafe_pwm[i];
+ }
+ pwm->channel_count = _max_actuators;
+ break;
+ }
+
+ case PWM_SERVO_SET_DISARMED_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] > PWM_MAX) {
+ _disarmed_pwm[i] = PWM_MAX;
+ } else if (pwm->values[i] < PWM_MIN) {
+ _disarmed_pwm[i] = PWM_MIN;
+ } else {
+ _disarmed_pwm[i] = pwm->values[i];
+ }
+ }
+
+ /*
+ * update the counter
+ * this is needed to decide if disarmed PWM output should be turned on or not
+ */
+ _num_disarmed_set = 0;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ if (_disarmed_pwm[i] > 0)
+ _num_disarmed_set++;
+ }
+ break;
+ }
+ case PWM_SERVO_GET_DISARMED_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _disarmed_pwm[i];
+ }
+ pwm->channel_count = _max_actuators;
+ break;
+ }
+
+ case PWM_SERVO_SET_MIN_PWM: {
+ struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] > PWM_HIGHEST_MIN) {
+ _min_pwm[i] = PWM_HIGHEST_MIN;
+ } else if (pwm->values[i] < PWM_MIN) {
+ _min_pwm[i] = PWM_MIN;
+ } else {
+ _min_pwm[i] = pwm->values[i];
+ }
+ }
+ break;
+ }
+ case PWM_SERVO_GET_MIN_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _min_pwm[i];
+ }
+ pwm->channel_count = _max_actuators;
+ arg = (unsigned long)&pwm;
+ break;
+ }
+
+ case PWM_SERVO_SET_MAX_PWM: {
+ struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+ /* discard if too many values are sent */
+ if (pwm->channel_count > _max_actuators) {
+ ret = -EINVAL;
+ break;
+ }
+ for (unsigned i = 0; i < pwm->channel_count; i++) {
+ if (pwm->values[i] == 0) {
+ /* ignore 0 */
+ } else if (pwm->values[i] < PWM_LOWEST_MAX) {
+ _max_pwm[i] = PWM_LOWEST_MAX;
+ } else if (pwm->values[i] > PWM_MAX) {
+ _max_pwm[i] = PWM_MAX;
+ } else {
+ _max_pwm[i] = pwm->values[i];
+ }
+ }
+ break;
+ }
+ case PWM_SERVO_GET_MAX_PWM: {
+ struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ pwm->values[i] = _max_pwm[i];
+ }
+ pwm->channel_count = _max_actuators;
+ arg = (unsigned long)&pwm;
+ break;
+ }
+
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
@@ -711,7 +908,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
- if (arg < 2100) {
+ if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
@@ -1093,6 +1290,20 @@ fmu_start(void)
return ret;
}
+int
+fmu_stop(void)
+{
+ int ret = OK;
+
+ if (g_fmu != nullptr) {
+
+ delete g_fmu;
+ g_fmu = nullptr;
+ }
+
+ return ret;
+}
+
void
test(void)
{
@@ -1136,7 +1347,7 @@ test(void)
}
} else {
// and use write interface for the other direction
- int ret = write(fd, servos, sizeof(servos));
+ 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);
}
@@ -1227,6 +1438,12 @@ fmu_main(int argc, char *argv[])
PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[1];
+ if (!strcmp(verb, "stop")) {
+ fmu_stop();
+ errx(0, "FMU driver stopped");
+ }
+
+
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index 05bc7a5b3..d918abd57 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -3,4 +3,5 @@
#
MODULE_COMMAND = fmu
-SRCS = fmu.cpp
+SRCS = fmu.cpp \
+ ../../modules/systemlib/pwm_limit/pwm_limit.c
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index aec118705..63e775857 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -184,21 +184,6 @@ public:
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
- * Set the minimum PWM signals when armed
- */
- int set_min_values(const uint16_t *vals, unsigned len);
-
- /**
- * Set the maximum PWM signal when armed
- */
- int set_max_values(const uint16_t *vals, unsigned len);
-
- /**
- * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
- */
- int set_idle_values(const uint16_t *vals, unsigned len);
-
- /**
* Disable RC input handling
*/
int disable_rc_handling();
@@ -953,55 +938,6 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
}
-int
-PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
-{
- if (len > _max_actuators)
- /* fail with error */
- return E2BIG;
-
- /* copy values to registers in IO */
- return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_min_values(const uint16_t *vals, unsigned len)
-{
-
- if (len > _max_actuators)
- /* fail with error */
- return E2BIG;
-
- /* copy values to registers in IO */
- return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_max_values(const uint16_t *vals, unsigned len)
-{
-
- if (len > _max_actuators)
- /* fail with error */
- return E2BIG;
-
- /* copy values to registers in IO */
- return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
-{
-
- if (len > _max_actuators)
- /* fail with error */
- return E2BIG;
-
- printf("Sending IDLE values\n");
-
- /* copy values to registers in IO */
- return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
-}
-
int
PX4IO::io_set_arming_state()
@@ -1874,10 +1810,10 @@ PX4IO::print_status()
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
- printf("\nidle values");
+ printf("\ndisarmed values");
for (unsigned i = 0; i < _max_actuators; i++)
- printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
+ printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
printf("\n");
}
@@ -1910,12 +1846,22 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
+ case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
+ /* get the default update rate */
+ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
+ break;
+
case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break;
- case PWM_SERVO_SELECT_UPDATE_RATE: {
+ case PWM_SERVO_GET_UPDATE_RATE:
+ /* get the alternative update rate */
+ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
+ break;
+
+ case PWM_SERVO_SET_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);
@@ -1934,6 +1880,87 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
}
+ case PWM_SERVO_GET_SELECT_UPDATE_RATE:
+
+ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
+ break;
+
+ case PWM_SERVO_SET_FAILSAFE_PWM: {
+ struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+ if (pwm->channel_count > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
+ break;
+ }
+
+ case PWM_SERVO_GET_FAILSAFE_PWM:
+
+ ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
+ if (ret != OK) {
+ ret = -EIO;
+ }
+ break;
+
+ case PWM_SERVO_SET_DISARMED_PWM: {
+ struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+ if (pwm->channel_count > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
+ break;
+ }
+
+ case PWM_SERVO_GET_DISARMED_PWM:
+
+ ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
+ if (ret != OK) {
+ ret = -EIO;
+ }
+ break;
+
+ case PWM_SERVO_SET_MIN_PWM: {
+ struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+ if (pwm->channel_count > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
+ break;
+ }
+
+ case PWM_SERVO_GET_MIN_PWM:
+
+ ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
+ if (ret != OK) {
+ ret = -EIO;
+ }
+ break;
+
+ case PWM_SERVO_SET_MAX_PWM: {
+ struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+ if (pwm->channel_count > _max_actuators)
+ /* fail with error */
+ return E2BIG;
+
+ /* copy values to registers in IO */
+ ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
+ break;
+ }
+
+ case PWM_SERVO_GET_MAX_PWM:
+
+ ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
+ if (ret != OK) {
+ ret = -EIO;
+ }
+ break;
+
case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _max_actuators;
break;
@@ -1958,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
/* TODO: we could go lower for e.g. TurboPWM */
unsigned channel = cmd - PWM_SERVO_SET(0);
- if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
+ if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) {
ret = -EINVAL;
} else {
@@ -2591,152 +2618,7 @@ px4io_main(int argc, char *argv[])
exit(0);
}
- if (!strcmp(argv[1], "failsafe")) {
-
- if (argc < 3) {
- errx(1, "failsafe command needs at least one channel value (ppm)");
- }
-
- /* set values for first 8 channels, fill unassigned channels with 1500. */
- uint16_t failsafe[8];
-
- for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
-
- /* set channel to commandline argument or to 900 for non-provided channels */
- if (argc > i + 2) {
- failsafe[i] = atoi(argv[i + 2]);
-
- if (failsafe[i] < 800 || failsafe[i] > 2200) {
- errx(1, "value out of range of 800 < value < 2200. Aborting.");
- }
-
- } else {
- /* a zero value will result in stopping to output any pulse */
- failsafe[i] = 0;
- }
- }
-
- int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
-
- if (ret != OK)
- errx(ret, "failed setting failsafe values");
-
- exit(0);
- }
-
- if (!strcmp(argv[1], "min")) {
-
- if (argc < 3) {
- errx(1, "min command needs at least one channel value (PWM)");
- }
-
- if (g_dev != nullptr) {
-
- /* set values for first 8 channels, fill unassigned channels with 900. */
- uint16_t min[8];
-
- for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) {
- /* set channel to commanline argument or to 900 for non-provided channels */
- if (argc > i + 2) {
- min[i] = atoi(argv[i + 2]);
-
- if (min[i] < 900 || min[i] > 1200) {
- errx(1, "value out of range of 900 < value < 1200. Aborting.");
- }
-
- } else {
- /* a zero value will the default */
- min[i] = 0;
- }
- }
-
- int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
-
- if (ret != OK)
- errx(ret, "failed setting min values");
-
- } else {
- errx(1, "not loaded");
- }
-
- exit(0);
- }
-
- if (!strcmp(argv[1], "max")) {
- if (argc < 3) {
- errx(1, "max command needs at least one channel value (PWM)");
- }
-
- if (g_dev != nullptr) {
-
- /* set values for first 8 channels, fill unassigned channels with 2100. */
- uint16_t max[8];
-
- for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) {
- /* set channel to commanline argument or to 2100 for non-provided channels */
- if (argc > i + 2) {
- max[i] = atoi(argv[i + 2]);
-
- if (max[i] < 1800 || max[i] > 2100) {
- errx(1, "value out of range of 1800 < value < 2100. Aborting.");
- }
-
- } else {
- /* a zero value will the default */
- max[i] = 0;
- }
- }
-
- int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
-
- if (ret != OK)
- errx(ret, "failed setting max values");
-
- } else {
- errx(1, "not loaded");
- }
-
- exit(0);
- }
-
- if (!strcmp(argv[1], "idle")) {
-
- if (argc < 3) {
- errx(1, "max command needs at least one channel value (PWM)");
- }
-
- if (g_dev != nullptr) {
-
- /* set values for first 8 channels, fill unassigned channels with 0. */
- uint16_t idle[8];
-
- for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) {
- /* set channel to commanline argument or to 0 for non-provided channels */
- if (argc > i + 2) {
- idle[i] = atoi(argv[i + 2]);
-
- if (idle[i] < 900 || idle[i] > 2100) {
- errx(1, "value out of range of 900 < value < 2100. Aborting.");
- }
-
- } else {
- /* a zero value will the default */
- idle[i] = 0;
- }
- }
-
- int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
-
- if (ret != OK)
- errx(ret, "failed setting idle values");
-
- } else {
- errx(1, "not loaded");
- }
-
- exit(0);
- }
if (!strcmp(argv[1], "recovery")) {
@@ -2808,5 +2690,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
}
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 236cb918d..43318ca84 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -48,6 +48,7 @@
#include <time.h>
#include <errno.h>
#include <string.h>
+#include <stdio.h>
#include <arch/board/board.h>
@@ -262,7 +263,8 @@ PX4IO_serial::init()
up_enable_irq(PX4IO_SERIAL_VECTOR);
/* enable UART in DMA mode, enable error and line idle interrupts */
- /* rCR3 = USART_CR3_EIE;*/
+ rCR3 = USART_CR3_EIE;
+
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
/* create semaphores */
@@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete()
DMA_SCR_PBURST_SINGLE |
DMA_SCR_MBURST_SINGLE);
stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+ //rCR1 &= ~USART_CR1_TE;
+ //rCR1 |= USART_CR1_TE;
rCR3 |= USART_CR3_DMAT;
perf_end(_pc_dmasetup);
- /* compute the deadline for a 5ms timeout */
+ /* compute the deadline for a 10ms timeout */
struct timespec abstime;
clock_gettime(CLOCK_REALTIME, &abstime);
-#if 0
- abstime.tv_sec++; /* long timeout for testing */
-#else
- abstime.tv_nsec += 10000000; /* 0ms timeout */
- if (abstime.tv_nsec > 1000000000) {
+ abstime.tv_nsec += 10*1000*1000;
+ if (abstime.tv_nsec >= 1000*1000*1000) {
abstime.tv_sec++;
- abstime.tv_nsec -= 1000000000;
+ abstime.tv_nsec -= 1000*1000*1000;
}
-#endif
/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
int ret;
@@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt()
*/
if (_rx_dma_status == _dma_status_waiting) {
_abort_dma();
- perf_count(_pc_uerrs);
+ perf_count(_pc_uerrs);
/* complete DMA as though in error */
_do_rx_dma_callback(DMA_STATUS_TEIF);
@@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt()
unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);
+
+ /* stop the receive DMA */
+ stm32_dmastop(_rx_dma);
+
+ /* complete the short reception */
+ _do_rx_dma_callback(DMA_STATUS_TEIF);
return;
}
@@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma()
stm32_dmastop(_rx_dma);
}
-#endif /* PX4IO_SERIAL_BASE */ \ No newline at end of file
+#endif /* PX4IO_SERIAL_BASE */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-16 06:33:47 +0000