From 30d851284606476642c6cf8a54070bc08ab923c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Nov 2014 12:31:04 +0100 Subject: MC pos control offb: read altitude sp separately --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522..f2c650ddd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt) _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } - if (_control_mode.flag_control_altitude_enabled) { + if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); -- cgit v1.2.3 From 59ec2401b6f8e6714d515d3d0f1cf2e0ee14b8bc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 00:48:27 +0100 Subject: fw pos control: better check for control mode --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) (limited to 'src/modules') 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 e7c95cc86..1bb27168e 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 @@ -909,10 +909,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float throttle_max = 1.0f; /* AUTONOMOUS FLIGHT */ - - // XXX this should only execute if auto AND safety off (actuators active), - // else integrators should be constantly reset. - if (pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_auto_enabled && + pos_sp_triplet.current.valid) { if (!_was_pos_control_mode) { /* reset integrators */ @@ -1248,7 +1246,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (_control_mode.flag_control_velocity_enabled) { + } else if (_control_mode.flag_control_velocity_enabled && + _control_mode.flag_control_altitude_enabled) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; if (!_was_velocity_control_mode) { -- cgit v1.2.3 From 5cd2ee83421e79c9f283d46b788343a479418d8a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 12:36:33 +0100 Subject: fw pos control: improve mode logic slightly --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 62 +++++++++++++--------- 1 file changed, 36 insertions(+), 26 deletions(-) (limited to 'src/modules') 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 1bb27168e..1592e70d8 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 @@ -200,9 +200,11 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; fwPosctrl::mTecs _mTecs; - bool _was_pos_control_mode; - bool _was_velocity_control_mode; - bool _was_alt_control_mode; + enum FW_POSCTRL_MODE { + FW_POSCTRL_MODE_AUTO, + FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_OTHER + } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. struct { float l1_period; @@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos_valid(false), _l1_control(), _mTecs(), - _was_pos_control_mode(false) + _control_mode_current(FW_POSCTRL_MODE_OTHER) { _nav_capabilities.turn_distance = 0.0f; @@ -908,20 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no throttle limit as default */ float throttle_max = 1.0f; - /* AUTONOMOUS FLIGHT */ if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + /* AUTONOMOUS FLIGHT */ - if (!_was_pos_control_mode) { + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } - - _was_pos_control_mode = true; - _was_velocity_control_mode = false; + _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ _hold_alt = _global_pos.alt; @@ -1248,35 +1249,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; - if (!_was_velocity_control_mode) { + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; - _was_alt_control_mode = false; } - _was_velocity_control_mode = true; - _was_pos_control_mode = false; + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_POSITION; + // Get demanded airspeed float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; // Get demanded vertical velocity from pitch control - float pitch = 0.0f; + static bool was_in_deadband = false; if (_manual.x > deadBand) { - pitch = (_manual.x - deadBand) / factor; - } else if (_manual.x < - deadBand) { - pitch = (_manual.x + deadBand) / factor; - } - if (pitch > 0.0f) { + float pitch = (_manual.x - deadBand) / factor; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - _was_alt_control_mode = false; - } else if (pitch < 0.0f) { + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - _was_alt_control_mode = false; - } else if (!_was_alt_control_mode) { + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; - _was_alt_control_mode = true; + was_in_deadband = true; } tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, @@ -1292,8 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed, TECS_MODE_NORMAL); } else { - _was_velocity_control_mode = false; - _was_pos_control_mode = false; + _control_mode_current = FW_POSCTRL_MODE_OTHER; /** MANUAL FLIGHT **/ -- cgit v1.2.3 From d0325f2b125a3573231443b7b4bac04b65081426 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 17:13:20 +0100 Subject: fw pos ctrl: takeoff special case only in takeoff --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 51 ++++++++++++---------- 1 file changed, 27 insertions(+), 24 deletions(-) (limited to 'src/modules') 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 1592e70d8..eec37f80c 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 @@ -1207,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else { /* Tell the attitude controller to stop integrating while we are waiting @@ -1290,18 +1290,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi was_in_deadband = true; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed, - TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1320,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } + /* Copy thrust output for publication */ if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auot + pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1336,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } -- cgit v1.2.3 From 6f89514beb09c5db3f9c71da22ceed7fe19e09c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 17:17:57 +0100 Subject: fix comment style and type --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules') 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 eec37f80c..167126a2b 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 @@ -1267,12 +1267,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _control_mode_current = FW_POSCTRL_MODE_POSITION; - // Get demanded airspeed + /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - // Get demanded vertical velocity from pitch control + /* Get demanded vertical velocity from pitch control */ static bool was_in_deadband = false; if (_manual.x > deadBand) { float pitch = (_manual.x - deadBand) / factor; @@ -1324,7 +1324,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auot + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, -- cgit v1.2.3 From 669e1f96d8fed848d2fbe71d3a10adb749a5fcc4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 10:46:36 +0100 Subject: fixed parameter issue for VTOL so that parameter wiki tool can document them --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 2 +- .../vtol_att_control/vtol_att_control_main.cpp | 2 +- .../vtol_att_control/vtol_att_control_params.c | 93 +++++++++++++++++++--- 3 files changed, 86 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 8f7a28b97..fb31142fa 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -13,4 +13,4 @@ set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 param set VTOL_MOT_COUNT 2 -param set IDLE_PWM_MC 1080 +param set VTOL_IDLE_PWM_MC 1080 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 24800cce8..de678a5b8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -238,7 +238,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; - _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); + _params_handles.idle_pwm_mc = param_find("VTOL_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 44157acba..6a234a3ba 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -1,13 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 vtol_att_control_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + #include -// number of engines +/** + * VTOL number of engines + * + * @min 1.0 + * @group VTOL Attitude Control + */ PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); -// idle pwm in multicopter mode -PARAM_DEFINE_INT32(IDLE_PWM_MC,900); -// min airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2); -// max airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30); -// trim airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10); + +/** + * Idle speed of VTOL when in multicopter mode + * + * @min 900 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); + +/** + * Minimum airspeed in multicopter mode + * + * This is the minimum speed of the air flowing over the control surfaces. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); + +/** + * Maximum airspeed in multicopter mode + * + * This is the maximum speed of the air flowing over the control surfaces. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); + +/** + * Trim airspeed when in multicopter mode + * + * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10.0f); -- cgit v1.2.3 From f3b5b67eec258476480a225b6b835e46335003b4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 12:43:23 +0100 Subject: shortened parameter names to max 16 bytes --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 4 ++-- src/modules/vtol_att_control/vtol_att_control_main.cpp | 10 +++++----- src/modules/vtol_att_control/vtol_att_control_params.c | 10 +++++----- 3 files changed, 12 insertions(+), 12 deletions(-) (limited to 'src/modules') diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index fb31142fa..7e9a6d3dc 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -12,5 +12,5 @@ set MIXER FMU_caipirinha_vtol set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 -param set VTOL_MOT_COUNT 2 -param set VTOL_IDLE_PWM_MC 1080 +param set VT_MOT_COUNT 2 +param set VT_IDLE_PWM_MC 1080 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index de678a5b8..958907d1b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -238,11 +238,11 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; - _params_handles.idle_pwm_mc = param_find("VTOL_IDLE_PWM_MC"); - _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); - _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); - _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); - _params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM"); + _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); + _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); + _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); + _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); + _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6a234a3ba..e21bccb0c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -46,7 +46,7 @@ * @min 1.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); +PARAM_DEFINE_INT32(VT_MOT_COUNT,0); /** * Idle speed of VTOL when in multicopter mode @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); * @min 900 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); /** * Minimum airspeed in multicopter mode @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); /** * Maximum airspeed in multicopter mode @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); /** * Trim airspeed when in multicopter mode @@ -84,5 +84,5 @@ PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); -- cgit v1.2.3 From 2a09473e5fc73bf2f59f0d27b0b5c38b2c91812f Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:26:31 +0000 Subject: topics: move geofence status to its own topic --- src/modules/commander/commander.cpp | 47 ++++++++++++++-------- src/modules/navigator/navigator.h | 11 +++++- src/modules/navigator/navigator_main.cpp | 24 ++++++++++-- src/modules/uORB/objects_common.cpp | 5 ++- src/modules/uORB/topics/geofence_result.h | 65 +++++++++++++++++++++++++++++++ src/modules/uORB/topics/mission_result.h | 11 +++--- 6 files changed, 135 insertions(+), 28 deletions(-) create mode 100644 src/modules/uORB/topics/geofence_result.h (limited to 'src/modules') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c01..9262f9e81 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[]) struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to geofence result topic */ + int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result)); + struct geofence_result_s geofence_result; + memset(&geofence_result, 0, sizeof(geofence_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1545,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } - /* Check for geofence violation */ - if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - static bool flight_termination_printed = false; - - if (!flight_termination_printed) { - warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); - flight_termination_printed = true; - } + /* start geofence result check */ + orb_check(geofence_result_sub, &updated); - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); - } - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + if (updated) { + orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); } + /* Check for geofence violation */ + if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + flight_termination_printed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9cd609955..a701e3f44 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include "navigator_mode.h" @@ -111,6 +112,11 @@ public: */ void publish_mission_result(); + /** + * Publish the geofence result + */ + void publish_geofence_result(); + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -134,6 +140,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } + struct geofence_result_s* get_geofence_result() { return &_geofence_result; } struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } @@ -164,6 +171,7 @@ private: orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; + orb_advert_t _geofence_result_pub; orb_advert_t _att_sp_pub; /**< publish att sp used only in very special failsafe modes when pos control is deactivated */ @@ -179,7 +187,8 @@ private: position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_result_s _mission_result; - vehicle_attitude_setpoint_s _att_sp; + geofence_result_s _geofence_result; + vehicle_attitude_setpoint_s _att_sp; bool _mission_item_valid; /**< flags if the current mission item is valid */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index df620e5e7..0ab85d56e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _geofence_result_pub(-1), _att_sp_pub(-1), _vstatus{}, _control_mode{}, @@ -398,8 +399,8 @@ Navigator::task_main() have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ - _mission_result.geofence_violated = true; - publish_mission_result(); + _geofence_result.geofence_violated = true; + publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -408,8 +409,8 @@ Navigator::task_main() } } else { /* inform other apps via the mission result */ - _mission_result.geofence_violated = false; - publish_mission_result(); + _geofence_result.geofence_violated = false; + publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } @@ -641,6 +642,21 @@ Navigator::publish_mission_result() } } +void +Navigator::publish_geofence_result() +{ + + /* lazily publish the geofence result only once available */ + if (_geofence_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result); + + } else { + /* advertise and publish */ + _geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result); + } +} + void Navigator::publish_att_sp() { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1141431cc..3d5755a46 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2014 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 @@ -148,6 +148,9 @@ ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" ORB_DEFINE(mission_result, struct mission_result_s); +#include "topics/geofence_result.h" +ORB_DEFINE(geofence_result, struct geofence_result_s); + #include "topics/fence.h" ORB_DEFINE(fence, unsigned); diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h new file mode 100644 index 000000000..b07e04499 --- /dev/null +++ b/src/modules/uORB/topics/geofence_result.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 geofence_result.h + * Status of the plance concerning the geofence + * + * @author Ban Siesta + */ + +#ifndef TOPIC_GEOFENCE_RESULT_H +#define TOPIC_GEOFENCE_RESULT_H + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct geofence_result_s +{ + bool geofence_violated; /**< true if the geofence is violated */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(geofence_result); + +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index c7d25d1f0..17ddaa01d 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier + * Copyright (C) 2012-2014 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 @@ -37,6 +34,11 @@ /** * @file mission_result.h * Mission results that navigator needs to pass on to commander and mavlink. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * @author Ban Siesta */ #ifndef TOPIC_MISSION_RESULT_H @@ -58,7 +60,6 @@ struct mission_result_s bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool geofence_violated; /**< true if the geofence is violated */ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; -- cgit v1.2.3 From 5bbd0743e082f247e3ff04e78e0e11b0583b9ff1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:27:25 +0000 Subject: navigator: deleted some verbose output --- src/modules/navigator/mission.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0765e9b7c..371480603 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -718,7 +718,6 @@ Mission::set_mission_item_reached() void Mission::set_current_offboard_mission_item() { - warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; -- cgit v1.2.3 From c9ca61ef5b23a370fcaf3e2a0546ab5452b65733 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:34:18 +0000 Subject: mavlink: don't slow mission updates down like this, otherwise we might miss mission results --- src/modules/mavlink/mavlink_mission.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index a3c127cdc..34c088778 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) void MavlinkMissionManager::send(const hrt_abstime now) { - /* update interval for slow rate limiter */ - _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); - bool updated = false; orb_check(_mission_result_sub, &updated); -- cgit v1.2.3 From f0ff914b626fbfb497143f80b19376efb524b9e1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:36:32 +0000 Subject: navigator: don't publish mission result immediately but only after every iteration of the navigator --- src/modules/navigator/datalinkloss.cpp | 8 ++++---- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator.h | 13 ++++++++----- src/modules/navigator/navigator_main.cpp | 6 ++++++ src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rcloss.cpp | 6 +++--- 7 files changed, 26 insertions(+), 17 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index e789fd10d..87a6e023a 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item() case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; @@ -188,7 +188,7 @@ DataLinkLoss::advance_dll() _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { @@ -209,7 +209,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: @@ -217,7 +217,7 @@ DataLinkLoss::advance_dll() warnx("time is up, state should have been changed manually by now"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index cd55f60b0..e370796c0 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 371480603..8de8faf9d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -711,7 +711,7 @@ Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } @@ -721,7 +721,7 @@ Mission::set_current_offboard_mission_item() _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); save_offboard_mission_state(); } @@ -730,5 +730,5 @@ void Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a701e3f44..d9d911d9c 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,11 +107,6 @@ public: */ void load_fence_from_file(const char *filename); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - /** * Publish the geofence result */ @@ -128,6 +123,7 @@ public: */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } + void set_mission_result_updated() { _mission_result_updated = true; } /** * Getters @@ -215,6 +211,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated; /**< flags if mission result has seen an update */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ @@ -280,6 +277,12 @@ private: */ void publish_position_setpoint_triplet(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ab85d56e..6a255878b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -139,6 +139,7 @@ Navigator::Navigator() : _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _pos_sp_triplet_published_invalid_once(false), + _mission_result_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -491,6 +492,11 @@ Navigator::task_main() _pos_sp_triplet_updated = false; } + if (_mission_result_updated) { + publish_mission_result(); + _mission_result_updated = false; + } + perf_end(_loop_perf); } warnx("exiting."); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3807c5ea8..2f322031c 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -65,7 +65,7 @@ NavigatorMode::run(bool active) { _first_run = false; /* Reset stay in failsafe flag */ _navigator->get_mission_result()->stay_in_failsafe = false; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 42392e739..a7cde6325 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -128,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -162,7 +162,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } break; @@ -171,7 +171,7 @@ RCLoss::advance_rcl() warnx("time is up, no RC regain, terminating"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: -- cgit v1.2.3 From 7cb4786e7934af50b5ceb370c593e27111d9dd17 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:38:10 +0000 Subject: navigator: report when a waypoint has been reached not when the whole mission is finished --- src/modules/navigator/mission.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8de8faf9d..3f0a6bb51 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #include @@ -149,18 +150,12 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + set_mission_item_reached(); if (_mission_item.autocontinue) { /* switch to next waypoint if 'autocontinue' flag set */ advance_mission(); set_mission_items(); - } else { - /* else just report that item reached */ - if (_mission_type == MISSION_TYPE_OFFBOARD) { - if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - set_mission_item_reached(); - } - } } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { @@ -395,7 +390,6 @@ Mission::set_mission_items() /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); - reset_mission_item_reached(); set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); -- cgit v1.2.3 From 180e17de335f337b17c0c411d0eb430cec760619 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:38:55 +0000 Subject: navigator: report using mission result if a DO_JUMP waypoint has been changed --- src/modules/mavlink/mavlink_mission.cpp | 8 +++++++- src/modules/navigator/mission.cpp | 12 ++++++++++++ src/modules/navigator/mission.h | 6 ++++++ src/modules/uORB/topics/mission_result.h | 3 +++ 4 files changed, 28 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 34c088778..859d380fe 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -309,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now) send_mission_current(_current_seq); + if (mission_result.item_do_jump_changed) { + /* send a mission item again if the remaining DO_JUMPs has changed */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, + (uint16_t)mission_result.item_changed_index); + } + } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); @@ -808,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; break; default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3f0a6bb51..9b0a092da 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -630,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s "ERROR DO JUMP waypoint could not be written"); return false; } + report_do_jump_mission_changed(*mission_index_ptr, + mission_item_tmp.do_jump_repeat_count); } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -700,6 +702,16 @@ Mission::save_offboard_mission_state() dm_unlock(DM_KEY_MISSION_STATE); } +void +Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + _navigator->set_mission_result_updated(); +} + void Mission::set_mission_item_reached() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ea7cc0927..a8a644b0f 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #ifndef NAVIGATOR_MISSION_H @@ -130,6 +131,11 @@ private: */ void save_offboard_mission_state(); + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + /** * Set a mission item as reached */ diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 17ddaa01d..2ddc529a3 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -61,6 +61,9 @@ struct mission_result_s bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ + bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ + unsigned item_changed_index; /**< indicate which item has changed */ + unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ }; /** -- cgit v1.2.3 From a4e27fd849610fe2dcd9426d0d5cb508e9563e97 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:39:25 +0000 Subject: navigator: reset some flags after the mission result has been published --- src/modules/navigator/navigator_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src/modules') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6a255878b..3f7670ec4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -646,6 +646,13 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } + + /* reset some of the flags */ + _mission_result.seq_reached = false; + _mission_result.seq_current = 0; + _mission_result.item_do_jump_changed = false; + _mission_result.item_changed_index = 0; + _mission_result.item_do_jump_remaining = 0; } void -- cgit v1.2.3 From a20b8d037bd91fddaebba7a61353c584dcf99314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:22:53 +0100 Subject: PX4IO firmware: Add required include. --- src/modules/px4iofirmware/protocol.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index c7e9ae3eb..b25a9507f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,6 +33,8 @@ #pragma once +#include + /** * @file protocol.h * -- cgit v1.2.3 From 752198352ebaa397caeb1c8cd18454231ffed389 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:32:43 +0100 Subject: IO firmware: Change to inttypes header --- src/modules/px4iofirmware/protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b25a9507f..bd777428f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,7 +33,7 @@ #pragma once -#include +#include /** * @file protocol.h -- cgit v1.2.3 From 54b68b73fd919d9fa376cca5297bb6ff6ea631f5 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 21 Dec 2014 09:03:18 +0530 Subject: Remove vel reset --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules') diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 296919c04..0af01cba1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1052,10 +1052,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } - /* reset xy velocity estimates when landed */ - x_est[1] = 0.0f; - y_est[1] = 0.0f; - } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { -- cgit v1.2.3 From b1f462a26638f252b0849083f1c3a81c52d49053 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 21 Dec 2014 14:09:04 +0000 Subject: geofence: don't fall over lines containing just a LF --- src/modules/navigator/geofence.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 0f431ded2..406137169 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename) while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; /* if the line starts with #, skip */ - if (line[textStart] == commentChar) + if (line[textStart] == commentChar) { continue; + } + + /* if there is only a linefeed, skip it */ + if (line[0] == '\n') { + continue; + } if (gotVertical) { /* Parse the line as a geofence point */ -- cgit v1.2.3 From 82383533c169e44ac769ce77f9e8ddfbd3082ed9 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 21 Dec 2014 14:09:30 +0000 Subject: geofence: be more verbose if import fails --- src/modules/navigator/geofence.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 406137169..4482fb36b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -297,8 +297,10 @@ Geofence::loadFromFile(const char *filename) /* Handle degree minute second format */ float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; - if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) + if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { + warnx("Scanf to parse DMS geofence vertex failed."); return ERROR; + } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); @@ -307,9 +309,10 @@ Geofence::loadFromFile(const char *filename) } else { /* Handle decimal degree format */ - - if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { + warnx("Scanf to parse geofence vertex failed."); return ERROR; + } } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) -- cgit v1.2.3 From 84d744707d07ae60dcf201a828d529057061fe47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 14:34:53 +0100 Subject: UAVCAN: Move into lib directory --- .gitmodules | 2 +- Tools/px4params/srcparser.py | 2 +- Tools/px4params/srcscanner.py | 7 ++++++- src/lib/uavcan | 1 + src/modules/uavcan/module.mk | 4 ++-- uavcan | 1 - 6 files changed, 11 insertions(+), 6 deletions(-) create mode 160000 src/lib/uavcan delete mode 160000 uavcan (limited to 'src/modules') diff --git a/.gitmodules b/.gitmodules index 4b84afac2..17a098431 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,5 +5,5 @@ path = NuttX url = git://github.com/PX4/NuttX.git [submodule "uavcan"] - path = uavcan + path = src/lib/uavcan url = git://github.com/pavel-kirienko/uavcan.git diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 0a4d21d26..8e6092195 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -103,7 +103,7 @@ class SourceParser(object): Returns list of supported file extensions that can be parsed by this parser. """ - return ["cpp", "c"] + return [".cpp", ".c"] def Parse(self, contents): """ diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index d7eca72d7..1f0ea4e89 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -26,5 +26,10 @@ class SourceScanner(object): parser.Parse method. """ with codecs.open(path, 'r', 'utf-8') as f: - contents = f.read() + try: + contents = f.read() + except: + contents = '' + print('Failed reading file: %s, skipping content.' % path) + pass parser.Parse(contents) diff --git a/src/lib/uavcan b/src/lib/uavcan new file mode 160000 index 000000000..1efd24427 --- /dev/null +++ b/src/lib/uavcan @@ -0,0 +1 @@ +Subproject commit 1efd24427539fa332a15151143466ec760fa5fff diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index f92bc754f..e5d30f6c4 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(UAVCAN_DIR)/libuavcan/include.mk +include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 diff --git a/uavcan b/uavcan deleted file mode 160000 index 1efd24427..000000000 --- a/uavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 1efd24427539fa332a15151143466ec760fa5fff -- cgit v1.2.3 From 0aaabaee0974aaca354e95f403cca3317fd1a83e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 15:02:01 +0100 Subject: VTOL: Remove unneeded headers --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'src/modules') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 958907d1b..9a80562f3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -74,11 +74,8 @@ #include #include "drivers/drv_pwm_output.h" -#include #include -#include - #include -- cgit v1.2.3 From 9292c8f405b0ed208443df0b1f9ebd497bb518ab Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 20 Dec 2014 10:27:02 -0700 Subject: add interrupt latency printout command and mean/variance to interval performance counter --- src/drivers/stm32/drv_hrt.c | 8 ++++-- src/modules/systemlib/perf_counter.c | 53 ++++++++++++++++++++++++++++-------- src/modules/systemlib/perf_counter.h | 9 +++++- src/systemcmds/perf/perf.c | 6 +++- 4 files changed, 60 insertions(+), 16 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 281f918d7..603c2eb9d 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -253,9 +253,11 @@ static uint16_t latency_baseline; static uint16_t latency_actual; /* latency histogram */ -#define LATENCY_BUCKET_COUNT 8 -static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + /* timer-specific functions */ static void hrt_tim_init(void); diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index d6d8284d2..f9e90652d 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -41,7 +41,7 @@ #include #include #include - +#include #include "perf_counter.h" /** @@ -84,7 +84,8 @@ struct perf_ctr_interval { uint64_t time_last; uint64_t time_least; uint64_t time_most; - + float mean; + float M2; }; /** @@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name) case PC_INTERVAL: ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; default: @@ -156,15 +158,23 @@ perf_count(perf_counter_t handle) case 1: pci->time_least = now - pci->time_last; pci->time_most = now - pci->time_last; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; break; default: { - hrt_abstime interval = now - pci->time_last; - if (interval < pci->time_least) - pci->time_least = interval; - if (interval > pci->time_most) - pci->time_most = interval; - break; - } + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } pci->time_last = now; pci->event_count++; @@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_INTERVAL: { 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\n", + 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); + pci->time_most, + (double)(1000 * pci->mean), + (double)(1000 * rms)); break; } @@ -365,6 +378,21 @@ perf_print_all(int fd) } } +extern const uint16_t latency_bucket_count; +extern uint32_t latency_counters[]; +extern const uint16_t latency_buckets[]; + +void +perf_print_latency(int fd) +{ + dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { + printf(" %4i : %i\n", latency_buckets[i], latency_counters[i]); + } + // print the overflow bucket value + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); +} + void perf_reset_all(void) { @@ -374,4 +402,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { + latency_counters[i] = 0; + } } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 668d9dfdf..d06606a5d 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -94,7 +94,7 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. - * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * If a call is made without a corresponding perf_begin call, or if perf_cancel * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. @@ -142,6 +142,13 @@ __EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle); */ __EXPORT extern void perf_print_all(int fd); +/** + * Print hrt latency counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +__EXPORT extern void perf_print_latency(int fd); + /** * Reset all of the performance counters. */ diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index b08a2e3b7..a788dfc11 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[]) if (strcmp(argv[1], "reset") == 0) { perf_reset_all(); return 0; + } else if (strcmp(argv[1], "latency") == 0) { + perf_print_latency(0 /* stdout */); + fflush(stdout); + return 0; } - printf("Usage: perf \n"); + printf("Usage: perf [reset | latency]\n"); return -1; } -- cgit v1.2.3