aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/bma180/bma180.cpp1
-rw-r--r--src/drivers/drv_accel.h1
-rw-r--r--src/drivers/drv_baro.h1
-rw-r--r--src/drivers/drv_gyro.h1
-rw-r--r--src/drivers/drv_mag.h1
-rw-r--r--src/drivers/drv_range_finder.h1
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp21
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp1
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp1
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp1
-rw-r--r--src/drivers/mb12xx/mb12xx.cpp1
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp4
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp2
-rw-r--r--src/drivers/ms5611/ms5611.cpp1
-rw-r--r--src/drivers/px4io/px4io.cpp221
15 files changed, 201 insertions, 58 deletions
diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index f0044d36f..1590cc182 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -690,6 +690,7 @@ BMA180::measure()
* measurement flow without using the external interrupt.
*/
report.timestamp = hrt_absolute_time();
+ report.error_count = 0;
/*
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 794de584b..eff5e7349 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -52,6 +52,7 @@
*/
struct accel_report {
uint64_t timestamp;
+ uint64_t error_count;
float x; /**< acceleration in the NED X board axis in m/s^2 */
float y; /**< acceleration in the NED Y board axis in m/s^2 */
float z; /**< acceleration in the NED Z board axis in m/s^2 */
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index 176f798c0..aa251889f 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -55,6 +55,7 @@ struct baro_report {
float altitude;
float temperature;
uint64_t timestamp;
+ uint64_t error_count;
};
/*
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 1d0fef6fc..fefcae50b 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -52,6 +52,7 @@
*/
struct gyro_report {
uint64_t timestamp;
+ uint64_t error_count;
float x; /**< angular velocity in the NED X board axis in rad/s */
float y; /**< angular velocity in the NED Y board axis in rad/s */
float z; /**< angular velocity in the NED Z board axis in rad/s */
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 3de5625fd..06107bd3d 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -54,6 +54,7 @@
*/
struct mag_report {
uint64_t timestamp;
+ uint64_t error_count;
float x;
float y;
float z;
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 03a82ec5d..cf91f7030 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -52,6 +52,7 @@
*/
struct range_finder_report {
uint64_t timestamp;
+ uint64_t error_count;
float distance; /** in meters */
uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */
};
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index dd8436b10..084671ae2 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -153,35 +153,36 @@ ETSAirspeed::collect()
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
- log("error reading from sensor: %d", ret);
+ perf_count(_comms_errors);
return ret;
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
if (diff_pres_pa == 0) {
- // a zero value means the pressure sensor cannot give us a
- // value. We need to return, and not report a value or the
- // caller could end up using this value as part of an
- // average
- log("zero value from sensor");
- return -1;
+ // a zero value means the pressure sensor cannot give us a
+ // value. We need to return, and not report a value or the
+ // caller could end up using this value as part of an
+ // average
+ perf_count(_comms_errors);
+ log("zero value from sensor");
+ return -1;
}
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
-
} else {
diff_pres_pa -= _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;
+ _max_differential_pressure_pa = diff_pres_pa;
}
// XXX we may want to smooth out the readings to remove noise.
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
+ report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = diff_pres_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -209,7 +210,7 @@ ETSAirspeed::cycle()
/* perform collection */
if (OK != collect()) {
- log("collection error");
+ perf_count(_comms_errors);
/* restart the measurement state machine */
start();
return;
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 5e891d7bb..5f0ce4ff8 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -791,6 +791,7 @@ HMC5883::collect()
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
+ new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 748809d3f..8f5674823 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -771,6 +771,7 @@ L3GD20::measure()
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
+ report.error_count = 0; // not recorded
switch (_orientation) {
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 2c56b6035..8bed8a8df 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1248,6 +1248,7 @@ LSM303D::measure()
accel_report.timestamp = hrt_absolute_time();
+ accel_report.error_count = 0; // not reported
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index ccc5bc15e..c910e2890 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -490,6 +490,7 @@ 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;
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 03d7bbfb9..ee45d46ac 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -138,7 +138,6 @@ MEASAirspeed::measure()
if (OK != ret) {
perf_count(_comms_errors);
- log("i2c::transfer returned %d", ret);
return ret;
}
@@ -161,7 +160,6 @@ MEASAirspeed::collect()
ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
- log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@@ -207,6 +205,7 @@ MEASAirspeed::collect()
}
report.timestamp = hrt_absolute_time();
+ report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.voltage = 0;
@@ -235,7 +234,6 @@ MEASAirspeed::cycle()
/* perform collection */
if (OK != collect()) {
- log("collection error");
/* restart the measurement state machine */
start();
return;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 14a3571de..70359110e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1186,7 +1186,7 @@ MPU6000::measure()
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
-
+ grb.error_count = arb.error_count = 0; // not reported
/*
* 1) Scale raw value to SI units using scaling from datasheet.
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 1c8a4d776..938786d3f 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -573,6 +573,7 @@ MS5611::collect()
struct baro_report report;
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
+ report.error_count = perf_event_count(_comms_errors);
/* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->read(0, (void *)&raw, 0);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f3fa3ed29..9c5e4f9dd 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -80,6 +80,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
#include <uORB/topics/parameter_update.h>
#include <debug.h>
@@ -197,6 +198,11 @@ public:
int set_idle_values(const uint16_t *vals, unsigned len);
/**
+ * Disable RC input handling
+ */
+ int disable_rc_handling();
+
+ /**
* Print IO status.
*
* Print all relevant IO status information
@@ -204,9 +210,9 @@ public:
void print_status();
/**
- * Disable RC input handling
+ * Fetch and print debug console output.
*/
- int disable_rc_handling();
+ int print_debug();
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/**
@@ -268,6 +274,7 @@ private:
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
+ orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
@@ -426,8 +433,27 @@ private:
*/
void dsm_bind_ioctl(int dsmMode);
-};
+ /**
+ * Handle a battery update from IO.
+ *
+ * Publish IO battery information if necessary.
+ *
+ * @param vbatt vbatt register
+ * @param ibatt ibatt register
+ */
+ void io_handle_battery(uint16_t vbatt, uint16_t ibatt);
+
+ /**
+ * Handle a servorail update from IO.
+ *
+ * Publish servo rail information if necessary.
+ *
+ * @param vservo vservo register
+ * @param vrssi vrssi register
+ */
+ void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
+};
namespace
{
@@ -463,6 +489,7 @@ PX4IO::PX4IO(device::Device *interface) :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
+ _to_servorail(0),
_to_safety(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
@@ -1232,10 +1259,70 @@ PX4IO::io_handle_alarms(uint16_t alarms)
return 0;
}
+void
+PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
+{
+ /* only publish if battery has a valid minimum voltage */
+ if (vbatt <= 3300) {
+ return;
+ }
+
+ battery_status_s battery_status;
+ battery_status.timestamp = hrt_absolute_time();
+
+ /* voltage is scaled to mV */
+ battery_status.voltage_v = vbatt / 1000.0f;
+
+ /*
+ ibatt contains the raw ADC count, as 12 bit ADC
+ value, with full range being 3.3v
+ */
+ battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
+ battery_status.current_a += _battery_amp_bias;
+
+ /*
+ integrate battery over time to get total mAh used
+ */
+ if (_battery_last_timestamp != 0) {
+ _battery_mamphour_total += battery_status.current_a *
+ (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+ }
+
+ battery_status.discharged_mah = _battery_mamphour_total;
+ _battery_last_timestamp = battery_status.timestamp;
+
+ /* lazily publish the battery voltage */
+ if (_to_battery > 0) {
+ orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+
+ } else {
+ _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+ }
+}
+
+void
+PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
+{
+ servorail_status_s servorail_status;
+ servorail_status.timestamp = hrt_absolute_time();
+
+ /* voltage is scaled to mV */
+ servorail_status.voltage_v = vservo * 0.001f;
+ servorail_status.rssi_v = vrssi * 0.001f;
+
+ /* lazily publish the servorail voltages */
+ if (_to_servorail > 0) {
+ orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+
+ } else {
+ _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+ }
+}
+
int
PX4IO::io_get_status()
{
- uint16_t regs[4];
+ uint16_t regs[6];
int ret;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
@@ -1247,41 +1334,13 @@ PX4IO::io_get_status()
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
- /* only publish if battery has a valid minimum voltage */
- if (regs[2] > 3300) {
- battery_status_s battery_status;
-
- battery_status.timestamp = hrt_absolute_time();
-
- /* voltage is scaled to mV */
- battery_status.voltage_v = regs[2] / 1000.0f;
-
- /*
- regs[3] contains the raw ADC count, as 12 bit ADC
- value, with full range being 3.3v
- */
- battery_status.current_a = regs[3] * (3.3f / 4096.0f) * _battery_amp_per_volt;
- battery_status.current_a += _battery_amp_bias;
-
- /*
- integrate battery over time to get total mAh used
- */
- if (_battery_last_timestamp != 0) {
- _battery_mamphour_total += battery_status.current_a *
- (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
- }
-
- battery_status.discharged_mah = _battery_mamphour_total;
- _battery_last_timestamp = battery_status.timestamp;
-
- /* lazily publish the battery voltage */
- if (_to_battery > 0) {
- orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+ io_handle_battery(regs[2], regs[3]);
+#endif
- } else {
- _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
- }
- }
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ io_handle_vservo(regs[4], regs[5]);
+#endif
return ret;
}
@@ -1528,8 +1587,54 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
}
int
+PX4IO::print_debug()
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+ int io_fd = -1;
+
+ if (io_fd < 0) {
+ io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
+ }
+
+ /* read IO's output */
+ if (io_fd > 0) {
+ pollfd fds[1];
+ fds[0].fd = io_fd;
+ fds[0].events = POLLIN;
+
+ usleep(500);
+ int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0);
+
+ if (pret > 0) {
+ int count;
+ char buf[65];
+
+ do {
+ count = ::read(io_fd, buf, sizeof(buf) - 1);
+
+ if (count > 0) {
+ /* enforce null termination */
+ buf[count] = '\0';
+ warnx("IO CONSOLE: %s", buf);
+ }
+
+ } while (count > 0);
+ }
+
+ ::close(io_fd);
+ return 0;
+ }
+
+#endif
+ return 1;
+
+}
+
+int
PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
{
+ /* get debug level */
+ int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG);
uint8_t frame[_max_transfer];
@@ -1551,6 +1656,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
+
+ } else {
+ continue;
}
/*
@@ -1569,6 +1677,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+ /* print mixer chunk */
+ if (debuglevel > 5 || ret) {
+
+ warnx("fmu sent: \"%s\"", msg->text);
+
+ /* read IO's output */
+ print_debug();
+ }
+
if (ret) {
log("mixer send error %d", ret);
return ret;
@@ -1578,6 +1695,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (buflen > 0);
+ /* ensure a closing newline */
+ msg->text[0] = '\n';
+ msg->text[1] = '\0';
+
+ int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+
retries--;
log("mixer sent");
@@ -2307,28 +2430,38 @@ test(void)
void
monitor(void)
{
+ /* clear screen */
+ printf("\033[2J");
+
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);
+ poll(fds, 1, 2000);
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
- if (cancels-- == 0)
+ if (cancels-- == 0) {
+ printf("\033[H"); /* move cursor home and clear screen */
exit(0);
+ }
}
-#warning implement this
+ if (g_dev != nullptr) {
+
+ printf("\033[H"); /* move cursor home and clear screen */
+ (void)g_dev->print_status();
+ (void)g_dev->print_debug();
+ printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
-// if (g_dev != nullptr)
-// g_dev->dump_one = true;
+ } else {
+ errx(1, "driver not loaded, exiting");
+ }
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-17 08:58:40 +0000