aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/batt_smbus/batt_smbus.cpp24
-rw-r--r--src/drivers/drv_range_finder.h3
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp4
-rw-r--r--src/drivers/mb12xx/mb12xx.cpp189
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp38
-rw-r--r--src/drivers/px4flow/i2c_frame.h82
-rw-r--r--src/drivers/px4flow/px4flow.cpp88
-rw-r--r--src/drivers/px4io/px4io.cpp4
-rw-r--r--src/drivers/px4io/px4io_uploader.cpp1
9 files changed, 308 insertions, 125 deletions
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index 2b5fef4d7..92b752a28 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -91,6 +91,7 @@
#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register
#define BATT_SMBUS_CURRENT 0x2a ///< current register
#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz
+#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup
#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
@@ -171,11 +172,13 @@ private:
uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
// internal variables
+ bool _enabled; ///< true if we have successfully connected to battery
work_s _work; ///< work queue for scheduling reads
RingBuffer *_reports; ///< buffer of recorded voltages, currents
struct battery_status_s _last_report; ///< last published report, used for test()
orb_advert_t _batt_topic; ///< uORB battery topic
orb_id_t _batt_orb_id; ///< uORB battery topic ID
+ uint64_t _start_time; ///< system time we first attempt to communicate with battery
};
namespace
@@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
+ _enabled(false),
_work{},
_reports(nullptr),
_batt_topic(-1),
- _batt_orb_id(nullptr)
+ _batt_orb_id(nullptr),
+ _start_time(0)
{
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
+
+ // capture startup time
+ _start_time = hrt_absolute_time();
}
BATT_SMBUS::~BATT_SMBUS()
@@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg)
void
BATT_SMBUS::cycle()
{
+ // get current time
+ uint64_t now = hrt_absolute_time();
+
+ // exit without rescheduling if we have failed to find a battery after 10 seconds
+ if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
+ warnx("did not find smart battery");
+ return;
+ }
+
// read data from sensor
struct battery_status_s new_report;
// set time of reading
- new_report.timestamp = hrt_absolute_time();
+ new_report.timestamp = now;
// read voltage
uint16_t tmp;
@@ -375,6 +392,9 @@ BATT_SMBUS::cycle()
// notify anyone waiting for data
poll_notify(POLLIN);
+
+ // record we are working
+ _enabled = true;
}
// schedule a fresh cycle call when the measurement is done
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 0f8362f58..12d51aeaa 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -45,6 +45,7 @@
#include "drv_orb_dev.h"
#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder"
+#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected
enum RANGE_FINDER_TYPE {
RANGE_FINDER_TYPE_LASER = 0,
@@ -67,6 +68,8 @@ struct range_finder_report {
float minimum_distance; /**< minimum distance the sensor can measure */
float maximum_distance; /**< maximum distance the sensor can measure */
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
+ float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */
+ uint8_t just_updated; /** number of the most recent measurement sensor */
};
/**
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 95fbed0ba..a06be72d9 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
}
int fd = open(bus.devpath, O_RDONLY);
- if (fd < 0)
+ if (fd < 0) {
return false;
+ }
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1,"Failed to setup poll rate");
}
+ close(fd);
return true;
}
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 9cf568658..a4fb7bcc2 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013-2015 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
@@ -34,6 +34,7 @@
/**
* @file mb12xx.cpp
* @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
*
* Driver for the Maxbotix sonar range finders connected via I2C.
*/
@@ -54,6 +55,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
+#include <vector>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
@@ -72,7 +74,7 @@
#include <board_config.h>
/* Configuration Constants */
-#define MB12XX_BUS PX4_I2C_BUS_EXPANSION
+#define MB12XX_BUS PX4_I2C_BUS_EXPANSION
#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */
#define MB12XX_DEVICE_PATH "/dev/mb12xx"
@@ -83,10 +85,12 @@
#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */
/* Device limits */
-#define MB12XX_MIN_DISTANCE (0.20f)
-#define MB12XX_MAX_DISTANCE (7.65f)
+#define MB12XX_MIN_DISTANCE (0.20f)
+#define MB12XX_MAX_DISTANCE (7.65f)
+
+#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */
-#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -133,12 +137,19 @@ private:
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
+ uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */
+ int _cycling_rate; /* */
+ uint8_t _index_counter; /* temporary sonar i2c address */
+ std::vector<uint8_t> addr_ind; /* temp sonar i2c address vector */
+ std::vector<float> _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+
+
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
- * @return True if the device is present.
+ * @return True if the device is present.
*/
int probe_address(uint8_t address);
@@ -178,7 +189,7 @@ private:
*
* @param arg Instance pointer for the driver that is polling.
*/
- static void cycle_trampoline(void *arg);
+ static void cycle_trampoline(void *arg);
};
@@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) :
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
- _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
+ _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
+ _cycle_counter(0), /* initialising counter for cycling function to zero */
+ _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */
+ _index_counter(0) /* initialising temp sonar i2c address to zero */
+
{
- // enable debug() calls
+ /* enable debug() calls */
_debug_enabled = false;
- // work_cancel in the dtor will explode if we don't do this...
+ /* work_cancel in the dtor will explode if we don't do this... */
memset(&_work, 0, sizeof(_work));
}
@@ -223,7 +238,7 @@ MB12XX::~MB12XX()
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
- // free perf counters
+ /* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
@@ -242,6 +257,9 @@ MB12XX::init()
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
+ _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
+ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
+
if (_reports == nullptr) {
goto out;
}
@@ -250,16 +268,51 @@ MB12XX::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
- struct range_finder_report rf_report;
- measure();
- _reports->get(&rf_report);
+ struct range_finder_report rf_report = {};
+
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
if (_range_finder_topic < 0) {
- debug("failed to create sensor_range_finder object. Did you start uOrb?");
+ log("failed to create sensor_range_finder object. Did you start uOrb?");
+ }
+ }
+
+ // XXX we should find out why we need to wait 200 ms here
+ usleep(200000);
+
+ /* check for connected rangefinders on each i2c port:
+ We start from i2c base address (0x70 = 112) and count downwards
+ So second iteration it uses i2c address 111, third iteration 110 and so on*/
+ for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
+ _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */
+ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
+ int ret2 = measure();
+
+ if (ret2 == 0) { /* sonar is present -> store address_index in array */
+ addr_ind.push_back(_index_counter);
+ debug("sonar added");
+ _latest_sonar_measurements.push_back(200);
}
}
+ _index_counter = MB12XX_BASEADDR;
+ set_address(_index_counter); /* set i2c port back to base adress for rest of driver */
+
+ /* if only one sonar detected, no special timing is required between firing, so use default */
+ if (addr_ind.size() == 1) {
+ _cycling_rate = MB12XX_CONVERSION_INTERVAL;
+
+ } else {
+ _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
+ }
+
+ /* show the connected sonars in terminal */
+ for (unsigned i = 0; i < addr_ind.size(); i++) {
+ log("sonar %d with address %d added", (i + 1), addr_ind[i]);
+ }
+
+ debug("Number of sonars connected: %d", addr_ind.size());
+
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
@@ -325,11 +378,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
- _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
+ _measure_ticks = USEC2TICK(_cycling_rate);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
+
}
return OK;
@@ -341,10 +395,10 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
- unsigned ticks = USEC2TICK(1000000 / arg);
+ int ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
- if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+ if (ticks < USEC2TICK(_cycling_rate)) {
return -EINVAL;
}
@@ -414,6 +468,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
MB12XX::read(struct file *filp, char *buffer, size_t buflen)
{
+
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0;
@@ -453,7 +508,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
}
/* wait for it to complete */
- usleep(MB12XX_CONVERSION_INTERVAL);
+ usleep(_cycling_rate * 2);
/* run the collection phase */
if (OK != collect()) {
@@ -474,17 +529,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
int
MB12XX::measure()
{
+
int ret;
/*
* Send the command to begin a measurement.
*/
+
uint8_t cmd = MB12XX_TAKE_RANGE_REG;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
- log("i2c::transfer returned %d", ret);
+ debug("i2c::transfer returned %d", ret);
return ret;
}
@@ -506,7 +563,7 @@ MB12XX::collect()
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
- log("error reading from sensor: %d", ret);
+ debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@@ -519,7 +576,39 @@ MB12XX::collect()
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
- report.distance = si_units;
+
+ /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
+ if (addr_ind.size() == 1) {
+ report.distance = si_units;
+
+ for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
+ report.distance_vector[i] = 0;
+ }
+
+ report.just_updated = 0;
+
+ } else {
+ /* for multiple sonars connected */
+
+ /* don't use the orginial single sonar variable */
+ report.distance = 0;
+
+ /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */
+ _latest_sonar_measurements[_cycle_counter] = si_units;
+
+ for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
+ report.distance_vector[i] = _latest_sonar_measurements[i];
+ }
+
+ /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */
+ report.just_updated = _index_counter;
+
+ /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
+ for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
+ report.distance_vector[addr_ind.size() + i] = 0;
+ }
+ }
+
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
@@ -545,12 +634,13 @@ MB12XX::collect()
void
MB12XX::start()
{
+
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
- work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
+ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
/* notify about state change */
struct subsystem_info_s info = {
@@ -564,8 +654,10 @@ MB12XX::start()
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
+
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
}
}
@@ -578,21 +670,24 @@ MB12XX::stop()
void
MB12XX::cycle_trampoline(void *arg)
{
+
MB12XX *dev = (MB12XX *)arg;
dev->cycle();
+
}
void
MB12XX::cycle()
{
- /* collection phase? */
if (_collect_phase) {
+ _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
+ set_address(_index_counter);
/* perform collection */
if (OK != collect()) {
- log("collection error");
- /* restart the measurement state machine */
+ debug("collection error");
+ /* if error restart the measurement state machine */
start();
return;
}
@@ -600,25 +695,37 @@ MB12XX::cycle()
/* next phase is measurement */
_collect_phase = false;
- /*
- * Is there a collect->measure gap?
- */
- if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
+ /* change i2c adress to next sonar */
+ _cycle_counter = _cycle_counter + 1;
+
+ if (_cycle_counter >= addr_ind.size()) {
+ _cycle_counter = 0;
+ }
+
+ /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
+ Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
+
+ if (_measure_ticks > USEC2TICK(_cycling_rate)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&MB12XX::cycle_trampoline,
this,
- _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
-
+ _measure_ticks - USEC2TICK(_cycling_rate));
return;
}
}
- /* measurement phase */
+ /* Measurement (firing) phase */
+
+ /* ensure sonar i2c adress is still correct */
+ _index_counter = addr_ind[_cycle_counter];
+ set_address(_index_counter);
+
+ /* Perform measurement */
if (OK != measure()) {
- log("measure error");
+ debug("measure error sonar adress %d", _index_counter);
}
/* next phase is collection */
@@ -629,7 +736,8 @@ MB12XX::cycle()
&_work,
(worker_t)&MB12XX::cycle_trampoline,
this,
- USEC2TICK(MB12XX_CONVERSION_INTERVAL));
+ USEC2TICK(_cycling_rate));
+
}
void
@@ -750,7 +858,7 @@ test()
}
warnx("single read");
- warnx("measurement: %0.2f m", (double)report.distance);
+ warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
@@ -779,7 +887,12 @@ test()
}
warnx("periodic read %u", i);
- warnx("measurement: %0.3f", (double)report.distance);
+
+ /* Print the sonar rangefinder report sonar distance vector */
+ for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) {
+ warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1);
+ }
+
warnx("time: %lld", report.timestamp);
}
@@ -830,7 +943,7 @@ info()
exit(0);
}
-} // namespace
+} /* namespace */
int
mb12xx_main(int argc, char *argv[])
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 168b34ea9..2be758244 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -921,26 +921,50 @@ MPU6000::gyro_self_test()
if (self_test())
return 1;
- /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
- if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+ /*
+ * Maximum deviation of 20 degrees, according to
+ * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
+ * Section 6.1, initial ZRO tolerance
+ */
+ const float max_offset = 0.34f;
+ /* 30% scale error is chosen to catch completely faulty units but
+ * to let some slight scale error pass. Requires a rate table or correlation
+ * with mag rotations + data fit to
+ * calibrate properly and is not done by default.
+ */
+ const float max_scale = 0.3f;
+
+ /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */
+ if (fabsf(_gyro_scale.x_offset) > max_offset)
return 1;
- if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+
+ /* evaluate gyro scale, complain if off by more than 30% */
+ if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale)
return 1;
- if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+ if (fabsf(_gyro_scale.y_offset) > max_offset)
return 1;
- if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+ if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale)
return 1;
- if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+ if (fabsf(_gyro_scale.z_offset) > max_offset)
return 1;
- if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+ if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale)
return 1;
+ /* check if all scales are zero */
+ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) &&
+ (fabsf(_gyro_scale.y_offset) < 0.000001f) &&
+ (fabsf(_gyro_scale.z_offset) < 0.000001f)) {
+ /* if all are zero, this device is not calibrated */
+ return 1;
+ }
+
return 0;
}
+
/*
perform a self-test comparison to factory trim values. This takes
about 200ms and will return OK if the current values are within 14%
diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h
new file mode 100644
index 000000000..b391b1f6a
--- /dev/null
+++ b/src/drivers/px4flow/i2c_frame.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013-2015 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file i2c_frame.h
+ * Definition of i2c frames.
+ * @author Thomas Boehm <thomas.boehm@fortiss.org>
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#ifndef I2C_FRAME_H_
+#define I2C_FRAME_H_
+#include <inttypes.h>
+
+
+typedef struct i2c_frame
+{
+ uint16_t frame_count;
+ int16_t pixel_flow_x_sum;
+ int16_t pixel_flow_y_sum;
+ int16_t flow_comp_m_x;
+ int16_t flow_comp_m_y;
+ int16_t qual;
+ int16_t gyro_x_rate;
+ int16_t gyro_y_rate;
+ int16_t gyro_z_rate;
+ uint8_t gyro_range;
+ uint8_t sonar_timestamp;
+ int16_t ground_distance;
+} i2c_frame;
+
+#define I2C_FRAME_SIZE (sizeof(i2c_frame))
+
+
+typedef struct i2c_integral_frame
+{
+ uint16_t frame_count_since_last_readout;
+ int16_t pixel_flow_x_integral;
+ int16_t pixel_flow_y_integral;
+ int16_t gyro_x_rate_integral;
+ int16_t gyro_y_rate_integral;
+ int16_t gyro_z_rate_integral;
+ uint32_t integration_timespan;
+ uint32_t sonar_timestamp;
+ uint16_t ground_distance;
+ int16_t gyro_temperature;
+ uint8_t qual;
+} i2c_integral_frame;
+
+#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame))
+
+#endif /* I2C_FRAME_H_ */
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 9c9c1b0f8..bb0cdbbb6 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -93,38 +93,11 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
-struct i2c_frame {
- uint16_t frame_count;
- int16_t pixel_flow_x_sum;
- int16_t pixel_flow_y_sum;
- int16_t flow_comp_m_x;
- int16_t flow_comp_m_y;
- int16_t qual;
- int16_t gyro_x_rate;
- int16_t gyro_y_rate;
- int16_t gyro_z_rate;
- uint8_t gyro_range;
- uint8_t sonar_timestamp;
- int16_t ground_distance;
-};
-struct i2c_frame f;
+#include "i2c_frame.h"
-struct i2c_integral_frame {
- uint16_t frame_count_since_last_readout;
- int16_t pixel_flow_x_integral;
- int16_t pixel_flow_y_integral;
- int16_t gyro_x_rate_integral;
- int16_t gyro_y_rate_integral;
- int16_t gyro_z_rate_integral;
- uint32_t integration_timespan;
- uint32_t time_since_last_sonar_update;
- uint16_t ground_distance;
- int16_t gyro_temperature;
- uint8_t qual;
-} __attribute__((packed));
+struct i2c_frame f;
struct i2c_integral_frame f_integral;
-
class PX4FLOW: public device::I2C
{
public:
@@ -150,8 +123,7 @@ private:
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
- bool _collect_phase;
-
+ bool _collect_phase;
orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf;
@@ -261,10 +233,10 @@ out:
int
PX4FLOW::probe()
{
- uint8_t val[22];
+ uint8_t val[I2C_FRAME_SIZE];
// to be sure this is not a ll40ls Lidar (which can also be on
- // 0x42) we check if a 22 byte transfer works from address
+ // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address
// 0. The ll40ls gives an error for that, whereas the flow
// happily returns some data
if (transfer(nullptr, 0, &val[0], 22) != OK) {
@@ -469,16 +441,16 @@ PX4FLOW::collect()
int ret = -EIO;
/* read from the sensor */
- uint8_t val[47] = { 0 };
+ uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
perf_begin(_sample_perf);
if (PX4FLOW_REG == 0x00) {
- ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
+ ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
- ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
+ ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
}
if (ret < 0) {
@@ -489,46 +461,12 @@ PX4FLOW::collect()
}
if (PX4FLOW_REG == 0) {
- 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];
- f.flow_comp_m_x = val[7] << 8 | val[6];
- f.flow_comp_m_y = val[9] << 8 | val[8];
- f.qual = val[11] << 8 | val[10];
- f.gyro_x_rate = val[13] << 8 | val[12];
- f.gyro_y_rate = val[15] << 8 | val[14];
- f.gyro_z_rate = val[17] << 8 | val[16];
- f.gyro_range = val[18];
- f.sonar_timestamp = val[19];
- f.ground_distance = val[21] << 8 | val[20];
-
- f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
- f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
- f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
- f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
- f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
- f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
- f_integral.integration_timespan = val[37] << 24 | val[36] << 16
- | val[35] << 8 | val[34];
- f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
- | val[39] << 8 | val[38];
- f_integral.ground_distance = val[43] << 8 | val[42];
- f_integral.gyro_temperature = val[45] << 8 | val[44];
- f_integral.qual = val[46];
+ memcpy(&f, val, I2C_FRAME_SIZE);
+ memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
- f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
- f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
- f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
- f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
- f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
- f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
- f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
- f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
- f_integral.ground_distance = val[21] << 8 | val[20];
- f_integral.gyro_temperature = val[23] << 8 | val[22];
- f_integral.qual = val[24];
+ memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
@@ -544,7 +482,7 @@ PX4FLOW::collect()
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
- report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
+ report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
@@ -828,7 +766,7 @@ test()
warnx("ground_distance: %0.2f m",
(double) f_integral.ground_distance / 1000);
warnx("time since last sonar update [us]: %i",
- f_integral.time_since_last_sonar_update);
+ f_integral.sonar_timestamp);
warnx("quality integration average : %i", f_integral.qual);
warnx("quality : %i", f.qual);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0451bbd1b..8ce548571 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2860,10 +2860,10 @@ checkcrc(int argc, char *argv[])
}
if (ret != OK) {
- printf("check CRC failed - %d\n", ret);
+ printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret);
exit(1);
}
- printf("CRCs match\n");
+ printf("[PX4IO::checkcrc] CRCs match\n");
exit(0);
}
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index fb16f891f..02e527695 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -621,6 +621,7 @@ int
PX4IO_Uploader::reboot()
{
send(PROTO_REBOOT);
+ up_udelay(100*1000); // Ensure the farend is in wait for char.
send(PROTO_EOC);
return OK;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-18 00:19:55 +0000