diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/bma180/bma180.cpp | 1 | ||||
-rw-r--r-- | src/drivers/drv_accel.h | 1 | ||||
-rw-r--r-- | src/drivers/drv_baro.h | 1 | ||||
-rw-r--r-- | src/drivers/drv_gyro.h | 1 | ||||
-rw-r--r-- | src/drivers/drv_mag.h | 1 | ||||
-rw-r--r-- | src/drivers/drv_range_finder.h | 1 | ||||
-rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 21 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 1 | ||||
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 1 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 1 | ||||
-rw-r--r-- | src/drivers/mb12xx/mb12xx.cpp | 1 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 4 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 2 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 1 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 221 |
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"); + } } } |