diff options
Diffstat (limited to 'src/modules')
| -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 |
10 files changed, 171 insertions, 63 deletions
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. |