From f7c3ed3ed601c981615f791e68aff68d119b7c54 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 15:00:34 +1000 Subject: px4io: split io_handle_battery() out from io_handle_status() ready to add vservo and rssi --- src/drivers/px4io/px4io.cpp | 86 +++++++++++++++++++++++++++------------------ 1 file changed, 52 insertions(+), 34 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63..af20e61cb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -409,9 +409,18 @@ private: */ int io_handle_alarms(uint16_t alarms); + /** + * Handle a battery update from IO. + * + * Publish IO battery information if necessary. + * + * @param vbatt vbattery register + * @param status ibatter register + */ + void io_handle_battery(uint16_t vbatt, uint16_t ibatt); + }; - namespace { @@ -1158,6 +1167,45 @@ 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); + } +} + int PX4IO::io_get_status() { @@ -1171,40 +1219,10 @@ 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); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + io_handle_battery(regs[2], regs[3]); +#endif return ret; } -- cgit v1.2.3 From f12794d30eb04d682c1977911752461e5c8a6eb8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 15:48:26 +1000 Subject: uORB: added new servorail_status object used for VSERVO and RSSI on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/servorail_status.h | 67 ++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 src/modules/uORB/topics/servorail_status.h (limited to 'src') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1eb63a799..3514dca24 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s); #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); +#include "topics/servorail_status.h" +ORB_DEFINE(servorail_status, struct servorail_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h new file mode 100644 index 000000000..55668790b --- /dev/null +++ b/src/modules/uORB/topics/servorail_status.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 + * 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 servorail_status.h + * + * Definition of the servorail status uORB topic. + */ + +#ifndef SERVORAIL_STATUS_H_ +#define SERVORAIL_STATUS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Servorail voltages and status + */ +struct servorail_status_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage_v; /**< Servo rail voltage in volts */ + float rssi_v; /**< RSSI pin voltage in volts */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(servorail_status); + +#endif -- cgit v1.2.3 From e9e46f9c9dc1301b4218903b26dbcd58fb096895 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 15:49:04 +1000 Subject: px4io: added monitoring of vservo and vrssi publish via servorail_status ORB topic --- src/drivers/px4io/px4io.cpp | 41 ++++++++++++++++++++++++++++++++--- src/modules/px4iofirmware/registers.c | 30 ++++++++++++------------- 2 files changed, 52 insertions(+), 19 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index af20e61cb..27b6a12da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -260,6 +261,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; /// 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 */ @@ -1224,6 +1255,10 @@ PX4IO::io_get_status() io_handle_battery(regs[2], regs[3]); #endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + io_handle_vservo(regs[4], regs[5]); +#endif + return ret; } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9c95fd1c5..30ef0ccea 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -678,27 +678,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #ifdef ADC_VSERVO /* PX4IO_P_STATUS_VSERVO */ { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * XXX pending measurements - * - * slope = xxx - * intercept = xxx - * - * Intercept corrected for best results @ 5.0V. - */ unsigned counts = adc_measure(ADC_VSERVO); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; } } #endif - /* XXX PX4IO_P_STATUS_VRSSI */ /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); -- cgit v1.2.3 From 0b7294a26ec33f707a9b5ddeee4269552b147e8b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 16:20:54 +1000 Subject: added error_count field to sensor report structures --- src/drivers/drv_accel.h | 1 + src/drivers/drv_baro.h | 1 + src/drivers/drv_gyro.h | 1 + src/drivers/drv_mag.h | 1 + src/drivers/drv_range_finder.h | 1 + src/modules/uORB/topics/differential_pressure.h | 1 + 6 files changed, 6 insertions(+) (limited to 'src') 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/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 1ffeda764..e4d2c92ce 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -53,6 +53,7 @@ */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t error_count; uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ -- cgit v1.2.3 From 7257642371f52a96c8d891795d090119437ea933 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 16:21:24 +1000 Subject: perf: added perf_event_count() method this allows drivers to get an event_count from a perf counter --- src/modules/systemlib/perf_counter.c | 26 ++++++++++++++++++++++++++ src/modules/systemlib/perf_counter.h | 8 ++++++++ 2 files changed, 34 insertions(+) (limited to 'src') diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 3c1e10287..bf84b7945 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle) } } +uint64_t +perf_event_count(perf_counter_t handle) +{ + if (handle == NULL) + return 0; + + switch (handle->type) { + case PC_COUNT: + return ((struct perf_ctr_count *)handle)->event_count; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } + + default: + break; + } + return 0; +} + void perf_print_all(void) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 4cd8b67a1..e1e3cbe95 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void); */ __EXPORT extern void perf_reset_all(void); +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +__EXPORT extern uint64_t perf_event_count(perf_counter_t handle); + __END_DECLS #endif -- cgit v1.2.3 From 4893509344f9304060dabc8a9ecad28fe891dcf8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Sep 2013 16:21:59 +1000 Subject: drivers: report error_count in drivers where possible --- src/drivers/bma180/bma180.cpp | 1 + src/drivers/ets_airspeed/ets_airspeed.cpp | 21 +++++++++++---------- src/drivers/hmc5883/hmc5883.cpp | 1 + src/drivers/l3gd20/l3gd20.cpp | 1 + src/drivers/lsm303d/lsm303d.cpp | 1 + src/drivers/mb12xx/mb12xx.cpp | 1 + src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +--- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/ms5611/ms5611.cpp | 1 + 9 files changed, 19 insertions(+), 14 deletions(-) (limited to 'src') 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/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 58a1593ed..de043db64 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 4c3b0ce51..ad6de0ab1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -774,6 +774,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 a90cd0a3d..7244019b1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1252,6 +1252,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); -- cgit v1.2.3 From d1871bd7bb9ae9eefdf1ada56fc3f57428e689eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Oct 2013 14:18:28 +0200 Subject: State machine fixes for HIL --- src/modules/commander/commander.cpp | 50 ++++++++++++++++---------- src/modules/commander/state_machine_helper.cpp | 29 +++++++++------ src/modules/commander/state_machine_helper.h | 2 +- src/modules/mavlink/waypoints.c | 4 ++- src/modules/sensors/sensors.cpp | 3 ++ 5 files changed, 56 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d0..0c3546b95 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off) { + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -532,6 +544,9 @@ static struct actuator_armed_s armed; static struct safety_s safety; +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - - /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[]) // warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[]) critical_voltage_counter++; } else { + low_voltage_counter = 0; critical_voltage_counter = 0; } @@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f..efe12be57 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,9 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + switch (new_arming_state) { case ARMING_STATE_INIT: @@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED) { + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; break; case HIL_STATE_ON: if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1641b6f60..0bfdf36a8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 97472c233..f03fe4fdf 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -307,7 +307,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; - if (!global_pos->valid && !local_pos->xy_valid) { + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { /* nothing to check here, return */ return; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c..9ef0d3c83 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { -- cgit v1.2.3 From 81e9c06129ff96508377777ab3a405977c873357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Oct 2013 21:04:59 +0200 Subject: Robustified flight close to waypoints --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 16 ++++++++++++++++ src/lib/ecl/l1/ecl_l1_pos_controller.h | 8 ++++++++ 2 files changed, 24 insertions(+) (limited to 'src') diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 87eb99f16..0dadf56f3 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { + /* this follows the logic presented in [1] */ float eta; @@ -116,6 +117,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate the vector from waypoint A to the aircraft */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector2f vector_B_to_airplane = get_local_planar_vector(vector_B, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -126,6 +128,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c * If the aircraft is already between A and B normal L1 logic is applied. */ float distance_A_to_airplane = vector_A_to_airplane.length(); + float distance_B_to_airplane = vector_B_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; /* extension from [2] */ @@ -143,6 +146,19 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } else if (distance_B_to_airplane < alongTrackDist || distance_B_to_airplane < _L1_distance) { + + /* fly directly to B */ + /* unit vector from waypoint B to current position */ + math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + } else { /* calculate eta to fly along the line between A and B */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index a6a2c0156..5a17346cb 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -130,6 +130,14 @@ public: bool reached_loiter_target(); + /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { + return _circle_mode; + } + + /** * Get the switch distance * -- cgit v1.2.3 From 5bc7d7c00f1f572825ce0db5f7fb24b8d77872a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:13:41 +0200 Subject: Fixed turn radius return value --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 0dadf56f3..c5c0c7a3c 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); + return math::max(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) @@ -117,7 +117,6 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate the vector from waypoint A to the aircraft */ math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - math::Vector2f vector_B_to_airplane = get_local_planar_vector(vector_B, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -128,10 +127,13 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c * If the aircraft is already between A and B normal L1 logic is applied. */ float distance_A_to_airplane = vector_A_to_airplane.length(); - float distance_B_to_airplane = vector_B_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; - /* extension from [2] */ + /* estimate airplane position WRT to B */ + math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + + /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { /* calculate eta to fly to waypoint A */ @@ -146,19 +148,21 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); - } else if (distance_B_to_airplane < alongTrackDist || distance_B_to_airplane < _L1_distance) { +// XXX this can be useful as last-resort guard, but is currently not needed +#if 0 + } else if (absf(bearing_wp_b) > math::radians(80.0f)) { + /* extension, fly back to waypoint */ - /* fly directly to B */ - /* unit vector from waypoint B to current position */ - math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized(); + /* calculate eta to fly to waypoint B */ + /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); - + _nav_bearing = bearing_wp_b; +#endif } else { /* calculate eta to fly along the line between A and B */ @@ -178,6 +182,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c eta = eta1 + eta2; /* bearing from current position to L1 point */ _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + } /* limit angle to +-90 degrees */ -- cgit v1.2.3 From d59cdf6fcc99e85cc1d897637b1bf9e18269f77c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:14:55 +0200 Subject: Added support for dynamic turn radii --- src/modules/mavlink/mavlink.c | 8 ++--- src/modules/mavlink/missionlib.c | 33 +++++++++++++----- src/modules/mavlink/orb_listener.c | 21 ++++++++++++ src/modules/mavlink/orb_topics.h | 5 +++ src/modules/mavlink/waypoints.c | 40 +++++++++++++++------- src/modules/mavlink/waypoints.h | 3 +- .../uORB/topics/vehicle_global_position_setpoint.h | 2 ++ 7 files changed, 86 insertions(+), 26 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7a5c2dab2..7c10e297b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); if (baudrate > 57600) { mavlink_pm_queued_send(); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 3175e64ce..e8d707948 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->param2 = param2; sp->param3 = param3; sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index cec2fdc43..0e0ce2723 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; @@ -122,6 +123,7 @@ static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); +static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -148,6 +150,7 @@ static const struct listener listeners[] = { {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, + {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -691,6 +694,19 @@ l_airspeed(const struct listener *l) mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } +void +l_nav_cap(const struct listener *l) +{ + + orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + hrt_absolute_time() / 1000, + "turn dist", + nav_cap.turn_distance); + +} + static void * uorb_receive_thread(void *arg) { @@ -837,6 +853,11 @@ uorb_receive_start(void) mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index c5cd0d03e..91773843b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -67,6 +67,7 @@ #include #include #include +#include struct mavlink_subscriptions { int sensor_sub; @@ -92,6 +93,7 @@ struct mavlink_subscriptions { int rates_setpoint_sub; int home_sub; int airspeed_sub; + int navigation_capabilities_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos; /** Local position */ extern struct vehicle_local_position_s local_pos; +/** navigation capabilities */ +extern struct navigation_capabilities_s nav_cap; + /** Vehicle status */ extern struct vehicle_status_s v_status; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index f03fe4fdf..ddad5f0df 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) * * The distance calculation is based on the WGS84 geoid (GPS) */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) { - static uint16_t counter; if (seq < wpm->size) { mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); @@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float float dxy = radius_earth * c; float dz = alt - wp->z; + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; } - counter++; - } /* * Calculate distance in local frame (NED) */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); @@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + return sqrtf(dx * dx + dy * dy + dz * dz); } else { @@ -303,7 +306,7 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float } } -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) { static uint16_t counter; @@ -332,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ orbit = 15.0f; } + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < turn_distance) + orbit = turn_distance; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } - if (dist >= 0.f && dist <= orbit) { + if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wpm->pos_reached = true; } + // check if required yaw reached float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); @@ -465,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -488,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - check_waypoints_reached(now, global_position, local_position); + check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -1011,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } - check_waypoints_reached(now, global_pos, local_pos); + // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 96a0ecd30..d7d6b31dc 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -55,6 +55,7 @@ #include #include #include +#include // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); + struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 5c8ce1e4d..a56434d3b 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s float param2; float param3; float param4; + float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ + float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ }; /** -- cgit v1.2.3 From a3bdf536e5f6b95d54ef6684d7092ebff2d23dc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Oct 2013 09:17:58 +0200 Subject: Dynamic integral resets for straight and circle mode, announcing turn radius now --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 +++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 31 +++++++++++++++++++++- .../uORB/topics/vehicle_attitude_setpoint.h | 2 ++ 3 files changed, 36 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6b98003fd..ae9aaa2da 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -593,6 +593,10 @@ FixedwingAttitudeControl::task_main() pitch_sp = _att_sp.pitch_body; throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) + _roll_ctrl.reset_integrator(); + } else { /* * Scale down roll and pitch as the setpoints are radians diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3d5bce134..cd4a0d58e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -129,9 +130,11 @@ private: int _accel_sub; /**< body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ @@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false) { + _nav_capabilities.turn_distance = 0.0f; + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); @@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { @@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* - * No valid next waypoint, go for heading hold. + * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ prev_wp.setX(global_triplet.current.lat / 1e7f); @@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -968,6 +982,21 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + } } diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 686fd765c..1a245132a 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s float thrust; /**< Thrust in Newton the power system should generate */ + bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + }; /** -- cgit v1.2.3 From d63ad0fb817d76d2d818db47805b46742d2aae68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Oct 2013 09:26:18 +0200 Subject: Added debug output printing capabilities for IOv2 --- src/drivers/px4io/px4io.cpp | 88 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 80 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7f67d02b5..38e183608 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -193,6 +193,11 @@ public: */ int set_idle_values(const uint16_t *vals, unsigned len); + /** + * Disable RC input handling + */ + int disable_rc_handling(); + /** * Print IO status. * @@ -201,9 +206,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 /** @@ -1531,9 +1536,53 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t return io_reg_set(page, offset, value); } +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); + } + + /* 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]), 10); + + 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]; @@ -1572,6 +1621,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; @@ -1581,6 +1639,11 @@ 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, 1); + retries--; log("mixer sent"); @@ -2242,28 +2305,37 @@ test(void) void monitor(void) { + /* clear screen */ + printf("\033[2J"); + unsigned cancels = 3; - printf("Hit 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) { -// if (g_dev != nullptr) -// g_dev->dump_one = true; + printf("\033[H"); /* move cursor home and clear screen */ + (void)g_dev->print_status(); + (void)g_dev->print_debug(); + printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + } else { + errx(1, "driver not loaded, exiting"); + } } } -- cgit v1.2.3 From ed00567400bd6ce24e25dc1038ce40f959205a68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Oct 2013 22:23:10 +0200 Subject: Extended file test for alignment --- src/systemcmds/tests/tests_file.c | 75 ++++++++++++++++++++++----------------- 1 file changed, 43 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index f36c28061..372bc5c90 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -63,24 +63,38 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t buf[512]; + uint8_t write_buf[512 + 64]; + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[512 + 64]; hrt_abstime start, end; perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - warnx("aligned write - please wait.."); - - if ((0x3 & (uintptr_t)buf)) - warnx("memory is unaligned!"); + warnx("testing aligned and unaligned writes - please wait.."); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); - write(fd, buf, sizeof(buf)); + int wret = write(fd, write_buf + (i % 64), 512); + + if (wret != 512) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + (i % 64)))) + warnx("memory is unaligned, align shift: %d", (i % 64)); + + } + fsync(fd); perf_end(wperf); + } end = hrt_absolute_time(); @@ -89,39 +103,36 @@ test_file(int argc, char *argv[]) perf_print_counter(wperf); perf_free(wperf); - int ret = unlink("/fs/microsd/testfile"); - - if (ret) - err(1, "UNLINKING FILE FAILED"); + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, 512); + + /* compare value */ + + for (int j = 0; j < 512; j++) { + if (read_buf[j] != write_buf[j + (i % 64)]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); + } + } - warnx("unaligned write - please wait.."); + } - struct { - uint8_t byte; - uint8_t unaligned[512]; - } unaligned_buf; + /* read back data for alignment checks */ + // for (unsigned i = 0; i < iterations; i++) { + // perf_begin(wperf); + // int rret = read(fd, buf + (i % 64), sizeof(buf)); + // fsync(fd); + // perf_end(wperf); - if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0) - warnx("creating unaligned memory failed."); + // } - wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)"); + int ret = unlink("/fs/microsd/testfile"); - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned)); - fsync(fd); - perf_end(wperf); - } - end = hrt_absolute_time(); + if (ret) + err(1, "UNLINKING FILE FAILED"); close(fd); - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - perf_print_counter(wperf); - perf_free(wperf); - /* list directory */ DIR *d; struct dirent *dir; -- cgit v1.2.3 From 1ca718b57fe0af737b0d2fb4e903162e5bb56852 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:03:57 +0200 Subject: Slight fix to test, but still fails validating written data after non-aligned writes --- src/systemcmds/tests/tests_file.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 372bc5c90..e9db4716d 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -103,18 +103,35 @@ test_file(int argc, char *argv[]) perf_print_counter(wperf); perf_free(wperf); + close(fd); + + fd = open("/fs/microsd/testfile", O_RDONLY); + /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { int rret = read(fd, read_buf, 512); + + if (rret != 512) { + warn("READ ERROR!"); + break; + } /* compare value */ + bool compare_ok = true; for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j + (i % 64)]) { + if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); + compare_ok = false; + break; } } + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + break; + } + } /* read back data for alignment checks */ -- cgit v1.2.3 From 13b07efc49584ea8b864403ff15bae9042ffb3af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:12:39 +0200 Subject: added hw test, added better io debugging --- makefiles/config_px4fmu-v2_default.mk | 3 ++ src/drivers/px4io/px4io.cpp | 9 +++-- src/examples/hwtest/hwtest.c | 74 +++++++++++++++++++++++++++++++++++ src/examples/hwtest/module.mk | 40 +++++++++++++++++++ 4 files changed, 123 insertions(+), 3 deletions(-) create mode 100644 src/examples/hwtest/hwtest.c create mode 100644 src/examples/hwtest/module.mk (limited to 'src') diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e12d61a53..66216f8fe 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -128,6 +128,9 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Hardware test +MODULES += examples/hwtest + # # Transitional support - add commands from the NuttX export archive. # diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 969dbefea..f6d4f1ed4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1545,7 +1545,7 @@ PX4IO::print_debug() int io_fd = -1; if (io_fd < 0) { - io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); } /* read IO's output */ @@ -1555,7 +1555,7 @@ PX4IO::print_debug() fds[0].events = POLLIN; usleep(500); - int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 10); + int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0); if (pret > 0) { int count; @@ -1606,6 +1606,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) memcpy(&msg->text[0], buf, count); buf += count; buflen -= count; + } else { + continue; } /* @@ -1644,7 +1646,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) /* ensure a closing newline */ msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, 1); + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); retries--; diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c new file mode 100644 index 000000000..06337be32 --- /dev/null +++ b/src/examples/hwtest/hwtest.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 hwtest.c + * + * Simple functional hardware test. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include + +__EXPORT int ex_hwtest_main(int argc, char *argv[]); + +int ex_hwtest_main(int argc, char *argv[]) +{ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + + int i; + float rcvalue = -1.0f; + hrt_abstime stime; + + while (true) { + stime = hrt_absolute_time(); + while (hrt_absolute_time() - stime < 1000000) { + for (i=0; i<8; i++) + actuators.control[i] = rcvalue; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + } + warnx("servos set to %.1f", rcvalue); + rcvalue *= -1.0f; + } + + return OK; +} diff --git a/src/examples/hwtest/module.mk b/src/examples/hwtest/module.mk new file mode 100644 index 000000000..6e70863ed --- /dev/null +++ b/src/examples/hwtest/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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 +# 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. +# +############################################################################ + +# +# Hardware test example application +# + +MODULE_COMMAND = ex_hwtest + +SRCS = hwtest.c -- cgit v1.2.3 From 21dcdf11cf8f05d94b17cd0b5cce058b45ee3923 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:14:03 +0200 Subject: WIP, typo fix --- src/systemcmds/tests/tests_file.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index e9db4716d..58eeb6fdf 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -82,7 +82,7 @@ test_file(int argc, char *argv[]) start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); - int wret = write(fd, write_buf + (i % 64), 512); + int wret = write(fd, write_buf + 1/*+ (i % 64)*/, 512); if (wret != 512) { warn("WRITE ERROR!"); @@ -109,7 +109,7 @@ test_file(int argc, char *argv[]) /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, 512); + int rret = read(fd, read_buf + 0, 512); if (rret != 512) { warn("READ ERROR!"); @@ -120,7 +120,7 @@ test_file(int argc, char *argv[]) bool compare_ok = true; for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { + if ((read_buf + 0)[j] != write_buf[j + 1/*+ (i % 64)*/]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); compare_ok = false; break; -- cgit v1.2.3 From 8407677f20ecc7ea5374a0ded41263ede019d9f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 00:15:39 +0200 Subject: Updated error message --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 58eeb6fdf..ab3dd7830 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -121,7 +121,7 @@ test_file(int argc, char *argv[]) for (int j = 0; j < 512; j++) { if ((read_buf + 0)[j] != write_buf[j + 1/*+ (i % 64)*/]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, (i % 64)); + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, 1/*(i % 64)*/); compare_ok = false; break; } -- cgit v1.2.3 From e0e708241b965f364fd49fa0d2270de5ad517a70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 08:34:08 +0200 Subject: More testing output --- src/systemcmds/tests/tests_file.c | 126 +++++++++++++++++++++++++++++++++----- 1 file changed, 112 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index ab3dd7830..aa0e163d7 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -77,7 +77,7 @@ test_file(int argc, char *argv[]) int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned and unaligned writes - please wait.."); + warnx("testing unaligned writes - please wait.."); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { @@ -104,12 +104,11 @@ test_file(int argc, char *argv[]) perf_free(wperf); close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); /* read back data for validation */ for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf + 0, 512); + int rret = read(fd, read_buf, 512); if (rret != 512) { warn("READ ERROR!"); @@ -120,7 +119,7 @@ test_file(int argc, char *argv[]) bool compare_ok = true; for (int j = 0; j < 512; j++) { - if ((read_buf + 0)[j] != write_buf[j + 1/*+ (i % 64)*/]) { + if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { warnx("COMPARISON ERROR: byte %d, align shift: %d", j, 1/*(i % 64)*/); compare_ok = false; break; @@ -134,22 +133,121 @@ test_file(int argc, char *argv[]) } - /* read back data for alignment checks */ - // for (unsigned i = 0; i < iterations; i++) { - // perf_begin(wperf); - // int rret = read(fd, buf + (i % 64), sizeof(buf)); - // fsync(fd); - // perf_end(wperf); - - // } + /* + * ALIGNED WRITES AND UNALIGNED READS + */ int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (ret) - err(1, "UNLINKING FILE FAILED"); + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + perf_begin(wperf); + int wret = write(fd, write_buf, 512); + + if (wret != 512) { + warn("WRITE ERROR!"); + } + + perf_end(wperf); + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, 512); + + if (rret != 512) { + warn("READ ERROR!"); + break; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < 512; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + break; + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + read_buf[0] = 0; + + warnx("printing first 10 pairs:"); + + uint8_t *read_ptr = &read_buf[1]; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_ptr, 512); + + warnx("first byte: %u (should be zero)", (unsigned int)read_buf[0]); + + if (rret != 512) { + warn("READ ERROR!"); + break; + } + + for (int j = 0; j < 512; j++) { + + if (i == 0 && j < 10) { + warnx("read: %u, expect: %u", (unsigned int)(read_buf + 1)[j], (unsigned int)write_buf[j]); + } + + if ((read_buf + 1)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + if (!unalign_read_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + break; + } + + } + + ret = unlink("/fs/microsd/testfile"); close(fd); + if (ret) + err(1, "UNLINKING FILE FAILED"); + /* list directory */ DIR *d; struct dirent *dir; -- cgit v1.2.3 From 6ffa2955b919a18abd94dd990c3447dfc68dd5c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 08:37:24 +0200 Subject: Typo in debug output --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index aa0e163d7..14532e48c 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -226,7 +226,7 @@ test_file(int argc, char *argv[]) } if ((read_buf + 1)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_ptr[j], (unsigned int)write_buf[j]); unalign_read_ok = false; unalign_read_err_count++; -- cgit v1.2.3 From 6745a910718b2a5fed313187f77a04de383aa8f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Oct 2013 08:42:54 +0200 Subject: Added alignment attribute --- src/systemcmds/tests/tests_file.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 14532e48c..3c0c48cfe 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -63,7 +63,7 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t write_buf[512 + 64]; + uint8_t write_buf[512 + 64] __attribute__((aligned(64))); /* fill write buffer with known values */ for (int i = 0; i < sizeof(write_buf); i++) { @@ -71,7 +71,7 @@ test_file(int argc, char *argv[]) write_buf[i] = i+11; } - uint8_t read_buf[512 + 64]; + uint8_t read_buf[512 + 64] __attribute__((aligned(64))); hrt_abstime start, end; perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); @@ -87,8 +87,8 @@ test_file(int argc, char *argv[]) if (wret != 512) { warn("WRITE ERROR!"); - if ((0x3 & (uintptr_t)(write_buf + (i % 64)))) - warnx("memory is unaligned, align shift: %d", (i % 64)); + if ((0x3 & (uintptr_t)(write_buf + 1 /* (i % 64)*/))) + warnx("memory is unaligned, align shift: %d", 1/*(i % 64)*/); } -- cgit v1.2.3 From 73f507daa6f31443e1938556c08aee016efaf345 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:12:30 +0200 Subject: Make tests file complete up to 64 byte shifts and 1.5 K chunks --- src/systemcmds/tests/tests_file.c | 270 +++++++++++++++++++------------------- 1 file changed, 133 insertions(+), 137 deletions(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 3c0c48cfe..a41d26bda 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -54,7 +54,8 @@ int test_file(int argc, char *argv[]) { - const iterations = 200; + const unsigned iterations = 100; + const unsigned alignments = 65; /* check if microSD card is mounted */ struct stat buffer; @@ -63,194 +64,189 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t write_buf[512 + 64] __attribute__((aligned(64))); + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; - /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { - /* this will wrap, but we just need a known value with spacing */ - write_buf[i] = i+11; - } + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - uint8_t read_buf[512 + 64] __attribute__((aligned(64))); - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + printf("\n====== FILE TEST: %u bytes chunks ======", chunk_sizes[c]); - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + for (unsigned a = 0; a < alignments; a++) { - warnx("testing unaligned writes - please wait.."); + warnx("----- alignment test: %u bytes -----", a); - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - int wret = write(fd, write_buf + 1/*+ (i % 64)*/, 512); + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - if (wret != 512) { - warn("WRITE ERROR!"); + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } - if ((0x3 & (uintptr_t)(write_buf + 1 /* (i % 64)*/))) - warnx("memory is unaligned, align shift: %d", 1/*(i % 64)*/); + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); - } + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - fsync(fd); - perf_end(wperf); + warnx("testing unaligned writes - please wait.."); - } - end = hrt_absolute_time(); + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); - perf_print_counter(wperf); - perf_free(wperf); + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + } - /* read back data for validation */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, 512); + fsync(fd); + //perf_end(wperf); - if (rret != 512) { - warn("READ ERROR!"); - break; - } - - /* compare value */ - bool compare_ok = true; - - for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j + 1/*+ (i % 64)*/]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d", j, 1/*(i % 64)*/); - compare_ok = false; - break; } - } + end = hrt_absolute_time(); - if (!compare_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - break; - } - - } + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - /* - * ALIGNED WRITES AND UNALIGNED READS - */ + //perf_print_counter(wperf); + //perf_free(wperf); - int ret = unlink("/fs/microsd/testfile"); - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); - warnx("testing aligned writes - please wait.."); + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - int wret = write(fd, write_buf, 512); + if (rret != chunk_sizes[c]) { + errx(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; - if (wret != 512) { - warn("WRITE ERROR!"); - } + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } - perf_end(wperf); + if (!compare_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } - } + } - fsync(fd); + /* + * ALIGNED WRITES AND UNALIGNED READS + */ - warnx("reading data aligned.."); + close(fd); + int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + warnx("testing aligned writes - please wait.."); - bool align_read_ok = true; + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); - /* read back data unaligned */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_buf, 512); + if (wret != chunk_sizes[c]) { + err(1, "WRITE ERROR!"); + } - if (rret != 512) { - warn("READ ERROR!"); - break; - } - - /* compare value */ - bool compare_ok = true; - - for (int j = 0; j < 512; j++) { - if (read_buf[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); - align_read_ok = false; - break; } - } - if (!align_read_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - break; - } + fsync(fd); - } + warnx("reading data aligned.."); - warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); - warnx("reading data unaligned.."); + bool align_read_ok = true; - close(fd); - fd = open("/fs/microsd/testfile", O_RDONLY); + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); - bool unalign_read_ok = true; - int unalign_read_err_count = 0; + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; - memset(read_buf, 0, sizeof(read_buf)); - read_buf[0] = 0; + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } - warnx("printing first 10 pairs:"); + if (!align_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } - uint8_t *read_ptr = &read_buf[1]; + } - /* read back data unaligned */ - for (unsigned i = 0; i < iterations; i++) { - int rret = read(fd, read_ptr, 512); + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); - warnx("first byte: %u (should be zero)", (unsigned int)read_buf[0]); + warnx("reading data unaligned.."); - if (rret != 512) { - warn("READ ERROR!"); - break; - } + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); - for (int j = 0; j < 512; j++) { + bool unalign_read_ok = true; + int unalign_read_err_count = 0; - if (i == 0 && j < 10) { - warnx("read: %u, expect: %u", (unsigned int)(read_buf + 1)[j], (unsigned int)write_buf[j]); - } + memset(read_buf, 0, sizeof(read_buf)); - if ((read_buf + 1)[j] != write_buf[j]) { - warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, 1/*(i % 64)*/, (unsigned int)read_ptr[j], (unsigned int)write_buf[j]); - unalign_read_ok = false; - unalign_read_err_count++; - - if (unalign_read_err_count > 10) - break; - } - } + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf + a, chunk_sizes[c]); - if (!unalign_read_ok) { - warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); - break; - } + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } - } + for (int j = 0; j < chunk_sizes[c]; j++) { - ret = unlink("/fs/microsd/testfile"); - close(fd); + if ((read_buf + a)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } - if (ret) - err(1, "UNLINKING FILE FAILED"); + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + } + } /* list directory */ - DIR *d; - struct dirent *dir; + DIR *d; + struct dirent *dir; d = opendir("/fs/microsd"); if (d) { -- cgit v1.2.3 From d144213527f9bdab92da9f81bb7647931314fa01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:13:54 +0200 Subject: Added non-binary number between 8 and 16 --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index a41d26bda..46e495f58 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -65,7 +65,7 @@ test_file(int argc, char *argv[]) } /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { -- cgit v1.2.3 From d04321bcf86d75f3f948998e871140d92967f1b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:14:57 +0200 Subject: Hotfix: Typo --- src/systemcmds/tests/tests_file.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 46e495f58..d8eac5efc 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -69,7 +69,7 @@ test_file(int argc, char *argv[]) for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - printf("\n====== FILE TEST: %u bytes chunks ======", chunk_sizes[c]); + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); for (unsigned a = 0; a < alignments; a++) { -- cgit v1.2.3 From 1306c9de7b946783ff1143bb42a33734e9380e2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Oct 2013 09:28:44 +0200 Subject: Output improvements --- src/systemcmds/tests/tests_file.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index d8eac5efc..7f3a654bf 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -73,6 +73,7 @@ test_file(int argc, char *argv[]) for (unsigned a = 0; a < alignments; a++) { + printf("\n"); warnx("----- alignment test: %u bytes -----", a); uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); -- cgit v1.2.3