diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 3bf02fbff3..fef02e6228 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -16,7 +16,7 @@ git checkout -b mydescriptivebranchname ### Edit and build the code -The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files. +The [developer guide](http://px4.io/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://px4.io/dev/code_style) when editing files. ### Commit your changes @@ -41,4 +41,4 @@ Since we care about safety, we will regularly ask you for test results. Best is Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/). -Make sure to provide some testing feedback and if possible the link to a flight log file. +Make sure to provide some testing feedback and if possible the link to a flight log file. Upload flight log files to [Log Muncher](http://dash.oznet.ch) and link the resulting report. diff --git a/README.md b/README.md index 1a559d3c3c..33b8cdec79 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ * Official Website: http://px4.io * License: BSD 3-clause (see LICENSE.md) -* Supported airframes: +* Supported airframes (more experimental are supported): * [Multicopters](http://px4.io/platforms/multicopters/start) * [Fixed wing](http://px4.io/platforms/planes/start) * [VTOL](http://px4.io/platforms/vtol/start) @@ -16,7 +16,7 @@ ### Users ### -Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with PX4. +Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with the PX4 flight stack. ### Developers ### @@ -26,9 +26,12 @@ http://px4.io/dev/contributing Developer guide: http://px4.io/dev/ +Testing guide: +http://px4.io/dev/unit_tests + This repository contains code supporting these boards: - * PX4FMUv1.x - * PX4FMUv2.x + * FMUv1.x + * FMUv2.x * AeroCore (v1 and v2) ## NuttShell (NSH) ## diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance index eeb9b48b29..e83a864d8d 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Long Range Quadcopter # -# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors +# Setup: 15 x 5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors # # Simon Wilks # @@ -11,21 +11,27 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then + param set BAT_N_CELLS 6 + param set BAT_V_EMPTY 3.5 + param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.07 + param set MC_ROLLRATE_P 0.08 param set MC_ROLLRATE_I 0.02 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.02 + param set MC_PITCHRATE_D 0.005 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.4 + param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + + param set MPC_XY_FF 0.2 + param set MPC_XY_VEL_MAX 2 fi set MIXER quad_w -set PWM_OUTPUTS 1234 -set PWM_MIN 1200 +set PWM_OUT 1234 +set PWM_MIN 1080 diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 87b98a02bc..5c00041492 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,4 +1,3 @@ -#!nsh # # Generic configuration file for caipirinha VTOL version # diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index b83687fbd4..0d6e0564af 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -252,9 +252,17 @@ fi # 13000 is historically reserved for the quadshot # -# VTOL caipririnha +# VTOL Caipririnha (Tailsitter) # - if param compare SYS_AUTOSTART 13001 - then - sh /etc/init.d/13001_caipirinha_vtol - fi +if param compare SYS_AUTOSTART 13001 +then + sh /etc/init.d/13001_caipirinha_vtol +fi + +# +# VTOL BirdsEyeView FireFly x6 (Tilt-Rotor) +# +if param compare SYS_AUTOSTART 13002 +then + sh /etc/init.d/13002_firefly6 +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 36a5124415..2c9387ff06 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -230,6 +230,7 @@ then tone_alarm $TUNE_ERR fi fi + unset IO_FILE if [ $IO_PRESENT == no ] then diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 2720806199..c46f6bede1 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -287,11 +287,12 @@ class uploader(object): #Draw progress bar (erase usually takes about 9 seconds to complete) estimatedTimeRemaining = deadline-time.time() - if estimatedTimeRemaining > 0: + if estimatedTimeRemaining >= 9.0: self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0) else: self.__drawProgressBar(10.0, 10.0) - sys.stdout.write(" (timeout: %d seconds) " % int(time.time()-deadline) ) + sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) + sys.stdout.flush() if self.__trySync(): self.__drawProgressBar(10.0, 10.0) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 743f20cdff..414eb06ddb 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,4 +1,5 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/msg/actuator_controls_0.msg b/msg/actuator_controls_0.msg index 743f20cdff..414eb06ddb 100644 --- a/msg/actuator_controls_0.msg +++ b/msg/actuator_controls_0.msg @@ -1,4 +1,5 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/msg/actuator_controls_virtual_mc.msg b/msg/actuator_controls_virtual_mc.msg index 743f20cdff..414eb06ddb 100644 --- a/msg/actuator_controls_virtual_mc.msg +++ b/msg/actuator_controls_virtual_mc.msg @@ -1,4 +1,5 @@ uint8 NUM_ACTUATOR_CONTROLS = 8 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fe70bd37fe..2a10b00632 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -80,9 +80,6 @@ * HMC5883 internal constants and data structures. */ -#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" -#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" - /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -114,9 +111,10 @@ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ enum HMC5883_BUS { - HMC5883_BUS_ALL, - HMC5883_BUS_INTERNAL, - HMC5883_BUS_EXTERNAL + HMC5883_BUS_ALL = 0, + HMC5883_BUS_I2C_INTERNAL, + HMC5883_BUS_I2C_EXTERNAL, + HMC5883_BUS_SPI }; /* oddly, ERROR is not defined for c++ */ @@ -1297,17 +1295,70 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int = nullptr; -HMC5883 *g_dev_ext = nullptr; +/* + list of supported bus configurations + */ +struct hmc5883_bus_option { + enum HMC5883_BUS busid; + const char *devpath; + HMC5883_constructor interface_constructor; + uint8_t busnum; + HMC5883 *dev; +} bus_options[] = { + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#ifdef PX4_I2C_BUS_ONBOARD + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_SPIDEV_HMC + { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(int bus, enum Rotation rotation); -void test(int bus); -void reset(int bus); -int info(int bus); -int calibrate(int bus); -const char* get_path(int bus); +void start(enum HMC5883_BUS busid, enum Rotation rotation); +bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation); +struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid); +void test(enum HMC5883_BUS busid); +void reset(enum HMC5883_BUS busid); +int info(enum HMC5883_BUS busid); +int calibrate(enum HMC5883_BUS busid); void usage(); +/** + * start driver for a specific bus option + */ +bool +start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) +{ + if (bus.dev != nullptr) + errx(1,"bus option already started"); + + device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + bus.dev = new HMC5883(interface, bus.devpath, rotation); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + if (fd < 0) + return false; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1,"Failed to setup poll rate"); + } + + return true; +} + + /** * Start the driver. * @@ -1315,151 +1366,58 @@ void usage(); * is either successfully up and running or failed to start. */ void -start(int external_bus, enum Rotation rotation) +start(enum HMC5883_BUS busid, enum Rotation rotation) { - int fd; + uint8_t i; + bool started = false; - /* create the driver, attempt expansion bus first */ - if (g_dev_ext != nullptr) { - warnx("already started external"); - } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { - - device::Device *interface = nullptr; - - /* create the driver, only attempt I2C for the external bus */ - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); - - if (interface->init() != OK) { - delete interface; - interface = nullptr; - warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION); - } + for (i=0; iinit() != OK) { - delete interface; - interface = nullptr; - warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD); - } - } -#endif - - /* interface will be null if init failed */ - if (interface != nullptr) { - - g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; - } - - } - } - - - /* if this failed, attempt onboard sensor */ - if (g_dev_int != nullptr) { - warnx("already started internal"); - } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { - - device::Device *interface = nullptr; - - /* create the driver, try SPI first, fall back to I2C if unsuccessful */ -#ifdef PX4_SPIDEV_HMC - if (HMC5883_SPI_interface != nullptr) { - interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS); - } -#endif - -#ifdef PX4_I2C_BUS_ONBOARD - /* this device is already connected as external if present above */ - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); - } -#endif - if (interface == nullptr) { - warnx("no internal bus scanned"); - goto fail; - } - - if (interface->init() != OK) { - delete interface; - warnx("no device on internal bus"); - } else { - - g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { - - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; - - if (external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } - } - if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } + if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; } + started |= start_bus(bus_options[i], rotation); } - if (g_dev_int == nullptr && g_dev_ext == nullptr) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - if (g_dev_int != nullptr) { - fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY); - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - close(fd); - } - - if (g_dev_ext != nullptr) { - fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY); - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - close(fd); - } + if (!started) + errx(1, "driver start failed"); + // one or more drivers started OK exit(0); - -fail: - if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) { - delete g_dev_int; - g_dev_int = nullptr; - } - if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) { - delete g_dev_ext; - g_dev_ext = nullptr; - } - - errx(1, "driver start failed"); } +/** + * find a bus structure for a busid + */ +struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) +{ + for (uint8_t i=0; iprint_info(); - ret = 0; - } - - return ret; -} - -const char* -get_path(int bus) -{ - return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); + warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); + bus.dev->print_info(); + exit(0); } void @@ -1650,22 +1595,25 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = HMC5883_BUS_ALL; + enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; - while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { + while ((ch = getopt(argc, argv, "XISR:C")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); break; #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = HMC5883_BUS_INTERNAL; + busid = HMC5883_BUS_I2C_INTERNAL; break; #endif case 'X': - bus = HMC5883_BUS_EXTERNAL; + busid = HMC5883_BUS_I2C_EXTERNAL; + break; + case 'S': + busid = HMC5883_BUS_SPI; break; case 'C': calibrate = true; @@ -1682,9 +1630,9 @@ hmc5883_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(verb, "start")) { - hmc5883::start(bus, rotation); + hmc5883::start(busid, rotation); if (calibrate) { - if (hmc5883::calibrate(bus) == 0) { + if (hmc5883::calibrate(busid) == 0) { errx(0, "calibration successful"); } else { @@ -1697,38 +1645,25 @@ hmc5883_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - hmc5883::test(bus); + hmc5883::test(busid); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - hmc5883::reset(bus); + hmc5883::reset(busid); /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == HMC5883_BUS_ALL) { - int ret = 0; - if (hmc5883::info(HMC5883_BUS_INTERNAL)) { - ret = 1; - } - - if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { - ret = 1; - } - exit(ret); - } else { - exit(hmc5883::info(bus)); - } - } + if (!strcmp(verb, "info") || !strcmp(verb, "status")) + hmc5883::info(busid); /* * Autocalibrate the scaling */ if (!strcmp(verb, "calibrate")) { - if (hmc5883::calibrate(bus) == 0) { + if (hmc5883::calibrate(busid) == 0) { errx(0, "calibration successful"); } else { diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index 0eb7736295..e91e91fc09 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,3 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; +typedef device::Device* (*HMC5883_constructor)(int); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6cac28a7db..168b34ea91 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -251,6 +251,8 @@ private: perf_counter_t _bad_registers; perf_counter_t _good_transfers; perf_counter_t _reset_retries; + perf_counter_t _system_latency_perf; + perf_counter_t _controller_latency_perf; uint8_t _register_wait; uint64_t _reset_wait; @@ -491,6 +493,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), + _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), _reset_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1731,6 +1735,9 @@ MPU6000::measure() _gyro->parent_poll_notify(); if (!(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + perf_begin(_system_latency_perf); /* publish it */ orb_publish(_accel_orb_id, _accel_topic, &arb); } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 58390ba4c3..96ebedd83c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -260,9 +260,9 @@ private: int _mavlink_fd; ///< mavlink file descriptor. - perf_counter_t _perf_update; ///current_state.arming_state; status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; status.hil_state = test->hil_state; + // The power status of the test unit is not relevant for the unit test + status.circuit_breaker_engaged_power_check = true; safety.safety_switch_available = test->safety_switch_available; safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; 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 d329ba2b08..e0b61e2e27 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1061,7 +1061,9 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = _att.timestamp; _actuators_airframe.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp_sample = _att.timestamp; /* publish the actuator controls */ if (_actuators_0_pub > 0) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dc031404de..9e4ab00df2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1286,30 +1286,11 @@ Mavlink::task_main(int argc, char *argv[]) } if (Mavlink::instance_exists(_device_name, this)) { - warnx("mavlink instance for %s already running", _device_name); + warnx("%s already running", _device_name); return ERROR; } - /* inform about mode */ - switch (_mode) { - case MAVLINK_MODE_NORMAL: - warnx("mode: NORMAL"); - break; - - case MAVLINK_MODE_CUSTOM: - warnx("mode: CUSTOM"); - break; - - case MAVLINK_MODE_ONBOARD: - warnx("mode: ONBOARD"); - break; - - default: - warnx("ERROR: Unknown mode"); - break; - } - - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1337,7 +1318,7 @@ Mavlink::task_main(int argc, char *argv[]) * marker ring buffer approach. */ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - errx(1, "can't allocate message buffer, exiting"); + errx(1, "msg buf:"); } /* initialize message buffer mutex */ @@ -1571,8 +1552,6 @@ Mavlink::task_main(int argc, char *argv[]) _subscriptions = nullptr; - warnx("waiting for UART receive thread"); - /* wait for threads to complete */ pthread_join(_receive_thread, NULL); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 859d380fe5..442d36dfba 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -120,13 +120,11 @@ MavlinkMissionManager::init_offboard_mission() _count = mission_state.count; _current_seq = mission_state.current_seq; - warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); - } else { _dataman_id = 0; _count = 0; _current_seq = 0; - warnx("offboard mission init: ERROR, reading mission state failed"); + warnx("offboard mission init: ERROR"); } } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 67ae90877d..228f142d97 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -141,6 +141,7 @@ private: struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _controller_latency_perf; math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ @@ -289,7 +290,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_circuit_breaker_enabled(false), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -886,10 +888,12 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = _v_att.timestamp; if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index abc69a4b50..d35b702391 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -199,6 +199,8 @@ static bool space_warning_sent = false; static pthread_t logwriter_pthread = 0; static pthread_attr_t logwriter_attr; +static perf_counter_t perf_write; + /** * Log buffer writing thread. Open and close file here. */ @@ -451,10 +453,10 @@ int open_log_file() int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); } return fd; @@ -515,8 +517,6 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); - int log_fd = open_log_file(); if (log_fd < 0) { @@ -620,16 +620,11 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - /* free performance counter */ - perf_free(perf_write); - return NULL; } void sdlog2_start_log() { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging"); - /* create log dir if needed */ if (create_log_dir() != 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); @@ -655,6 +650,9 @@ void sdlog2_start_log() logwriter_should_exit = false; + /* allocate write performance counter */ + perf_write = perf_alloc(PC_ELAPSED, "sd write"); + /* start log buffer emptying thread */ if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); @@ -674,8 +672,6 @@ void sdlog2_start_log() void sdlog2_stop_log() { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging"); - logging_enabled = false; /* wake up write thread one last time */ @@ -701,6 +697,11 @@ void sdlog2_stop_log() perf_print_all(perf_fd); close(perf_fd); + /* free log writer performance counter */ + perf_free(perf_write); + + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + sdlog2_status(); } diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index f9e90652d7..950577f003 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -67,10 +68,13 @@ struct perf_ctr_count { struct perf_ctr_elapsed { struct perf_ctr_header hdr; uint64_t event_count; + uint64_t event_overruns; uint64_t time_start; uint64_t time_total; uint64_t time_least; uint64_t time_most; + float mean; + float M2; }; /** @@ -126,6 +130,28 @@ perf_alloc(enum perf_counter_type type, const char *name) return ctr; } +perf_counter_t +perf_alloc_once(enum perf_counter_type type, const char *name) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + if (!strcmp(handle->name, name)) { + if (type == handle->type) { + /* they are the same counter */ + return handle; + } else { + /* same name but different type, assuming this is an error and not intended */ + return NULL; + } + } + handle = (perf_counter_t)sq_next(&handle->link); + } + + /* if the execution reaches here, no existing counter of that name was found */ + return perf_alloc(type, name); +} + void perf_free(perf_counter_t handle) { @@ -213,17 +239,72 @@ perf_end(perf_counter_t handle) struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; if (pce->time_start != 0) { - hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + int64_t elapsed = hrt_absolute_time() - pce->time_start; + + if (elapsed < 0) { + pce->event_overruns++; + } else { + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < (uint64_t)elapsed) + pce->time_most = elapsed; + + // maintain mean and variance of the elapsed time in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = elapsed / 1e6f; + float delta_intvl = dt - pce->mean; + pce->mean += delta_intvl / pce->event_count; + pce->M2 += delta_intvl * (dt - pce->mean); + + pce->time_start = 0; + } + } + } + break; + + default: + break; + } +} + +#include + +void +perf_set(perf_counter_t handle, int64_t elapsed) +{ + if (handle == NULL) { + return; + } + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + if (elapsed < 0) { + pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) pce->time_least = elapsed; - if (pce->time_most < elapsed) + if (pce->time_most < (uint64_t)elapsed) pce->time_most = elapsed; + // maintain mean and variance of the elapsed time in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = elapsed / 1e6f; + float delta_intvl = dt - pce->mean; + pce->mean += delta_intvl / pce->event_count; + pce->M2 += delta_intvl * (dt - pce->mean); + pce->time_start = 0; } } @@ -310,14 +391,17 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + float rms = sqrtf(pce->M2 / (pce->event_count-1)); - dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n", - handle->name, - pce->event_count, - pce->time_total, - pce->time_total / pce->event_count, - pce->time_least, - pce->time_most); + dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + pce->event_count, + pce->event_overruns, + pce->time_total, + pce->time_total / pce->event_count, + pce->time_least, + pce->time_most, + (double)(1e6f * rms)); break; } @@ -325,14 +409,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle) struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; float rms = sqrtf(pci->M2 / (pci->event_count-1)); - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n", - handle->name, - pci->event_count, - (pci->time_last - pci->time_first) / pci->event_count, - pci->time_least, - pci->time_most, - (double)(1000 * pci->mean), - (double)(1000 * rms)); + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + pci->event_count, + (pci->time_last - pci->time_first) / pci->event_count, + pci->time_least, + pci->time_most, + (double)(1e6f * rms)); break; } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 49590c0863..8543ba7bb6 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -57,7 +57,7 @@ typedef struct perf_ctr_header *perf_counter_t; __BEGIN_DECLS /** - * Create a new counter. + * Create a new local counter. * * @param type The type of the new counter. * @param name The counter name. @@ -66,6 +66,16 @@ __BEGIN_DECLS */ __EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); +/** + * Get the reference to an existing counter or create a new one if it does not exist. + * + * @param type The type of the counter. + * @param name The counter name. + * @return Handle for the counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name); + /** * Free a counter. * @@ -102,6 +112,18 @@ __EXPORT extern void perf_begin(perf_counter_t handle); */ __EXPORT extern void perf_end(perf_counter_t handle); +/** + * Register a measurement + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresponding perf_begin call. It sets the + * value provided as argument as a new measurement. + * + * @param handle The handle returned from perf_alloc. + * @param elapsed The time elapsed. Negative values lead to incrementing the overrun counter. + */ +__EXPORT extern void perf_set(perf_counter_t handle, int64_t elapsed); + /** * Cancel a performance event. *