aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/drv_pwm_output.h5
-rw-r--r--src/drivers/drv_px4flow.h31
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp20
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp37
-rw-r--r--src/drivers/px4flow/px4flow.cpp68
-rw-r--r--src/drivers/px4fmu/fmu.cpp4
6 files changed, 56 insertions, 109 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 84815fdfb..5aff6825b 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -95,6 +95,11 @@ __BEGIN_DECLS
#define PWM_LOWEST_MAX 1700
/**
+ * Do not output a channel with this value
+ */
+#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
+
+/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.
*/
diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h
index 76ec55c3e..ab640837b 100644
--- a/src/drivers/drv_px4flow.h
+++ b/src/drivers/drv_px4flow.h
@@ -46,37 +46,6 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Optical flow in NED body frame in SI units.
- *
- * @see http://en.wikipedia.org/wiki/International_System_of_Units
- *
- * @warning If possible the usage of the raw flow and performing rotation-compensation
- * using the autopilot angular rate estimate is recommended.
- */
-struct px4flow_report {
-
- uint64_t timestamp; /**< in microseconds since system start */
-
- int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
- int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
- float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
- float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
- float ground_distance_m; /**< Altitude / distance to ground in meters */
- uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
- uint8_t sensor_id; /**< id of the sensor emitting the flow value */
-
-};
-
-/**
- * @}
- */
-
/*
* ObjDev tag for px4flow data.
*/
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index f98d615a2..0f77bb805 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -155,7 +155,6 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
- uint16_t diff_pres_pa;
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
return -1;
}
- if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
- diff_pres_pa = 0;
- } else {
- diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
- }
-
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
- if (diff_pres_pa > _max_differential_pressure_pa) {
- _max_differential_pressure_pa = diff_pres_pa;
+ if (diff_pres_pa_raw > _max_differential_pressure_pa) {
+ _max_differential_pressure_pa = diff_pres_pa_raw;
}
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
- report.differential_pressure_pa = (float)diff_pres_pa;
// XXX we may want to smooth out the readings to remove noise.
- report.differential_pressure_filtered_pa = (float)diff_pres_pa;
- report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
+ report.differential_pressure_filtered_pa = diff_pres_pa_raw;
+ report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -369,7 +361,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
- warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+ warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
- warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+ warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
}
/* reset the sensor polling to its default rate */
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 159706278..1d9a463ad 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -228,44 +228,23 @@ MEASAirspeed::collect()
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
- float diff_press_pa = fabsf(diff_press_pa_raw);
-
/*
- note that we return both the absolute value with offset
- applied and a raw value without the offset applied. This
- makes it possible for higher level code to detect if the
- user has the tubes connected backwards, and also makes it
- possible to correctly use offsets calculated by a higher
- level airspeed driver.
-
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
-
- Also note that the _diff_pres_offset is applied before the
- fabsf() not afterwards. It needs to be done this way to
- prevent a bias at low speeds, but this also means that when
- setting a offset you must set it based on the raw value, not
- the offset value
*/
-
+
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
- if (diff_press_pa > _max_differential_pressure_pa) {
- _max_differential_pressure_pa = diff_press_pa;
+ if (diff_press_pa_raw > _max_differential_pressure_pa) {
+ _max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
- report.differential_pressure_pa = diff_press_pa;
- report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
-
- /* the dynamics of the filter can make it overshoot into the negative range */
- if (report.differential_pressure_filtered_pa < 0.0f) {
- report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
- }
+ report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -345,7 +324,7 @@ MEASAirspeed::cycle()
/**
correct for 5V rail voltage if the system_power ORB topic is
available
-
+
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
@@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
- temperature -= voltage_diff * temp_slope;
+ temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
@@ -523,7 +502,7 @@ test()
}
warnx("single read");
- warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+ warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -551,7 +530,7 @@ test()
}
warnx("periodic read %u", i);
- warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+ warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index f214b5964..60ad3c1af 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -37,7 +37,7 @@
*
* Driver for the PX4FLOW module connected via I2C.
*/
-
+
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -68,7 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
-//#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/optical_flow.h>
#include <board_config.h>
@@ -80,7 +80,7 @@
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
-#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
-
+
virtual int init();
-
+
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
-
+
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
-
+
protected:
virtual int probe();
@@ -136,13 +136,13 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
-
+
orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
-
+
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -151,7 +151,7 @@ private:
* @return True if the device is present.
*/
int probe_address(uint8_t address);
-
+
/**
* Initialise the automatic measurement state machine and start it.
*
@@ -159,12 +159,12 @@ private:
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
-
+
/**
* Stop the automatic measurement state machine.
*/
void stop();
-
+
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@@ -179,8 +179,8 @@ private:
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
-
-
+
+
};
/*
@@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
{
// enable debug() calls
_debug_enabled = true;
-
+
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@@ -226,13 +226,13 @@ PX4FLOW::init()
goto out;
/* allocate basic report buffers */
- _reports = new RingBuffer(2, sizeof(px4flow_report));
+ _reports = new RingBuffer(2, sizeof(struct optical_flow_s));
if (_reports == nullptr)
goto out;
/* get a publish handle on the px4flow topic */
- struct px4flow_report zero_report;
+ struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
@@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
-
+
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
-
+
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
-
+
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
-
+
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
- unsigned count = buflen / sizeof(struct px4flow_report);
- struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+ unsigned count = buflen / sizeof(struct optical_flow_s);
+ struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -425,7 +425,7 @@ PX4FLOW::measure()
return ret;
}
ret = OK;
-
+
return ret;
}
@@ -433,14 +433,14 @@ int
PX4FLOW::collect()
{
int ret = -EIO;
-
+
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
-
+
perf_begin(_sample_perf);
-
+
ret = transfer(nullptr, 0, &val[0], 22);
-
+
if (ret < 0)
{
log("error reading from sensor: %d", ret);
@@ -448,7 +448,7 @@ PX4FLOW::collect()
perf_end(_sample_perf);
return ret;
}
-
+
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
@@ -466,7 +466,7 @@ PX4FLOW::collect()
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
- struct px4flow_report report;
+ struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
@@ -503,7 +503,7 @@ PX4FLOW::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
-
+
/* notify about state change */
struct subsystem_info_s info = {
true,
@@ -644,7 +644,7 @@ start()
fail:
- if (g_dev != nullptr)
+ if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
@@ -678,7 +678,7 @@ void stop()
void
test()
{
- struct px4flow_report report;
+ struct optical_flow_s report;
ssize_t sz;
int ret;
@@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[])
*/
if (!strcmp(argv[1], "start"))
px4flow::start();
-
+
/*
* Stop the driver
*/
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 82977a032..122a3cd17 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
- up_pwm_servo_set(i, values[i]);
+ if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
+ up_pwm_servo_set(i, values[i]);
+ }
}
return count * 2;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 12:09:15 +0000