diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 309 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.h | 1 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 7 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 16 | ||||
-rw-r--r-- | src/examples/fixedwing_control/module.mk | 2 | ||||
m--------- | src/lib/uavcan | 0 | ||||
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 23 | ||||
-rw-r--r-- | src/modules/commander/commander_tests/state_machine_helper_test.cpp | 2 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 27 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_mission.cpp | 4 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 23 | ||||
-rw-r--r-- | src/modules/systemlib/perf_counter.c | 121 | ||||
-rw-r--r-- | src/modules/systemlib/perf_counter.h | 24 |
16 files changed, 308 insertions, 261 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fe70bd37f..2a10b0063 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,169 +1295,129 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int = nullptr; -HMC5883 *g_dev_ext = nullptr; - -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); +/* + 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(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 the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. + * start driver for a specific bus option */ -void -start(int external_bus, enum Rotation rotation) +bool +start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) { - int fd; - - /* 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); - } - } - -#ifdef PX4_I2C_BUS_ONBOARD - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); - - if (interface->init() != 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 (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 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 (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1,"Failed to setup poll rate"); + } - if (interface->init() != OK) { - delete interface; - warnx("no device on internal bus"); - } else { + return true; +} - 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; +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum HMC5883_BUS busid, enum Rotation rotation) +{ + uint8_t i; + bool started = false; - if (external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } - } - if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } + for (i=0; i<NUM_BUS_OPTIONS; i++) { + if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; } + 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; i<NUM_BUS_OPTIONS; i++) { + if ((busid == HMC5883_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + errx(1,"bus %u not started", (unsigned)busid); } + /** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void -test(int bus) +test(enum HMC5883_BUS busid) { + struct hmc5883_bus_option &bus = find_bus(busid); struct mag_report report; ssize_t sz; int ret; - const char *path = get_path(bus); + const char *path = bus.devpath; int fd = open(path, O_RDONLY); @@ -1557,10 +1515,11 @@ test(int bus) * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. * Using the self test method described above, the user can scale sensor */ -int calibrate(int bus) +int calibrate(enum HMC5883_BUS busid) { int ret; - const char *path = get_path(bus); + struct hmc5883_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; int fd = open(path, O_RDONLY); @@ -1585,9 +1544,10 @@ int calibrate(int bus) * Reset the driver. */ void -reset(int bus) +reset(enum HMC5883_BUS busid) { - const char *path = get_path(bus); + struct hmc5883_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; int fd = open(path, O_RDONLY); @@ -1607,28 +1567,13 @@ reset(int bus) * Print a little info about the driver. */ int -info(int bus) +info(enum HMC5883_BUS busid) { - int ret = 1; + struct hmc5883_bus_option &bus = find_bus(busid); - HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext); - if (g_dev == nullptr) { - warnx("not running on bus %d", bus); - } else { - - warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard")); - - g_dev->print_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 0eb773629..e91e91fc0 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 6cac28a7d..168b34ea9 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 58390ba4c..96ebedd83 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; ///<local performance counter for status updates - perf_counter_t _perf_write; ///<local performance counter for PWM control writes - perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes + perf_counter_t _perf_update; ///< local performance counter for status updates + perf_counter_t _perf_write; ///< local performance counter for PWM control writes + perf_counter_t _perf_sample_latency; ///< total system latency (based on passed-through timestamp) /* cached IO state */ uint16_t _status; ///< Various IO status flags @@ -493,7 +493,7 @@ PX4IO::PX4IO(device::Device *interface) : _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "io update")), _perf_write(perf_alloc(PC_ELAPSED, "io write")), - _perf_chan_count(perf_alloc(PC_COUNT, "io rc #")), + _perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")), _status(0), _alarms(0), _t_actuator_controls_0(-1), @@ -551,7 +551,7 @@ PX4IO::~PX4IO() /* deallocate perfs */ perf_free(_perf_update); perf_free(_perf_write); - perf_free(_perf_chan_count); + perf_free(_perf_sample_latency); g_dev = nullptr; } @@ -1111,6 +1111,7 @@ PX4IO::io_set_control_state(unsigned group) if (changed) { orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); + perf_set(_perf_sample_latency, hrt_elapsed_time(&controls.timestamp_sample)); } } break; @@ -1570,11 +1571,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) channel_count = RC_INPUT_MAX_CHANNELS; } - /* count channel count changes to identify signal integrity issues */ - if (channel_count != _rc_chan_count) { - perf_count(_perf_chan_count); - } - _rc_chan_count = channel_count; input_rc.timestamp_publication = hrt_absolute_time(); diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index f6c882ead..da96054d3 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wframe-larger-than=1200 +EXTRACFLAGS = -Wframe-larger-than=1300 diff --git a/src/lib/uavcan b/src/lib/uavcan -Subproject 1efd24427539fa332a15151143466ec760fa5ff +Subproject c4c45b995f5c8192c7a36c4293c201711ceac74 diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index f0e02c331..fd6bd0218 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -133,7 +133,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 7200, + 7700, attitude_estimator_ekf_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 086f291f6..f511f3876 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -739,9 +739,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); - /* welcome user */ - warnx("starting"); - const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; @@ -1246,6 +1243,7 @@ int commander_thread_main(int argc, char *argv[]) orb_check(safety_sub, &updated); if (updated) { + bool previous_safety_off = safety.safety_off; orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ @@ -1259,6 +1257,19 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } } + + //Notify the user if the status of the safety switch changes + if(safety.safety_switch_available && previous_safety_off != safety.safety_off) { + + if(safety.safety_off) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } + else { + tune_neutral(true); + } + + status_changed = true; + } } /* update vtol vehicle status*/ @@ -1952,6 +1963,12 @@ int commander_thread_main(int argc, char *argv[]) /* reset arm_tune_played when disarmed */ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { + + //Notify the user that it is safe to approach the vehicle + if(arm_tune_played) { + tune_neutral(true); + } + arm_tune_played = false; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 874090e93..3cfa8b4c6 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -280,6 +280,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) status.arming_state = test->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 d329ba2b0..e0b61e2e2 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 dc031404d..9e4ab00df 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 859d380fe..442d36dfb 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 67ae90877..228f142d9 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 abc69a4b5..d35b70239 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 f9e90652d..950577f00 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -39,6 +39,7 @@ #include <stdlib.h> #include <stdio.h> +#include <string.h> #include <sys/queue.h> #include <drivers/drv_hrt.h> #include <math.h> @@ -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 <systemlib/err.h> + +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; - - 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); + float rms = sqrtf(pce->M2 / (pce->event_count-1)); + + 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 49590c086..8543ba7bb 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. @@ -67,6 +67,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. * * @param handle The performance counter's handle. @@ -103,6 +113,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. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. |