From a46a1fe6560d72cf45171a20a6917c09b27e0454 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=82=A3=E4=B8=AAZeng?= Date: Mon, 4 Apr 2022 21:45:51 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=B8=80=E4=BA=9B=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E6=89=93=E5=8D=B0=E6=B6=88=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../FlightTaskManualAltitude.cpp | 28 ++++++++++++------ .../FlightTaskManualPosition.cpp | 29 ++++++++++++++++++- 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 7d08e11044..f3a769f5bc 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -107,6 +107,16 @@ void FlightTaskManualAltitude::_scaleSticks() // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); + + static uint32_t last_2s,delt_time; + uint32_t nowtime = hrt_absolute_time(); + if( (nowtime - last_2s) > 2 * 1000000ULL) + { + mavlink_log_info(&_mavlink_log_pub, "scaleSticks(%d) 0-3: %.2f,%.2f,%.2f,%.2f\n",(nowtime - delt_time),(double)_sticks.getPositionExpo()(0),(double)_sticks.getPositionExpo()(1),(double)_sticks.getPositionExpo()(2),(double)_sticks.getPositionExpo()(3)); + last_2s = nowtime; + } + delt_time = hrt_absolute_time(); + } float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target) @@ -378,15 +388,15 @@ bool FlightTaskManualAltitude::update() _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); - static uint32_t last_2s,delt_time; - uint32_t nowtime = hrt_absolute_time(); - if( (nowtime - last_2s) > 2 * 1000000ULL) - { - mavlink_log_info(&_mavlink_log_pub, "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U); - printf( "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U ); - last_2s = nowtime; - } - delt_time = hrt_absolute_time(); + // static uint32_t last_2s,delt_time; + // uint32_t nowtime = hrt_absolute_time(); + // if( (nowtime - last_2s) > 2 * 1000000ULL) + // { + // mavlink_log_info(&_mavlink_log_pub, "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U); + // printf( "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U ); + // last_2s = nowtime; + // } + // delt_time = hrt_absolute_time(); return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 06daff9b3c..c43a300d9c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -72,6 +72,15 @@ bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; _velocity_scale = _constraints.speed_xy; + static uint32_t last_2s,delt_time; + uint32_t nowtime = hrt_absolute_time(); + if( (nowtime - last_2s) > 2_s) + { + mavlink_log_info(&_mavlink_log_pub, "activate(%d) : %.2f,%.2f\n",(nowtime - delt_time),(double)_position_setpoint(0),(double)_position_setpoint(1)); + last_2s = nowtime; + } + delt_time = hrt_absolute_time(); + // for position-controlled mode, we need a valid position and velocity state // in NE-direction return ret; @@ -121,7 +130,7 @@ void FlightTaskManualPosition::_scaleSticks() uint32_t nowtime = hrt_absolute_time(); if( (nowtime - last_2s) > 2_s) { - mavlink_log_info(&_mavlink_log_pub, "scaleSticks(%d) : %.2f,%.2f\n",delt_time,(double)vel_sp_xy(0),(double)vel_sp_xy(1)); + mavlink_log_info(&_mavlink_log_pub, "scaleSticks(%d) : %.2f,%.2f\n",(nowtime - delt_time),(double)vel_sp_xy(0),(double)vel_sp_xy(1)); last_2s = nowtime; } delt_time = hrt_absolute_time(); @@ -152,6 +161,15 @@ void FlightTaskManualPosition::_updateXYlock() _position_setpoint(0) = NAN; _position_setpoint(1) = NAN; } + + static uint32_t last_2s,delt_time; + uint32_t nowtime = hrt_absolute_time(); + if( (nowtime - last_2s) > 2_s) + { + mavlink_log_info(&_mavlink_log_pub, "_updateXYlock(%d) : %.2f,%.2f\n",(nowtime - delt_time),(double)_velocity(0),(double)_velocity(1)); + last_2s = nowtime; + } + delt_time = hrt_absolute_time(); } void FlightTaskManualPosition::_updateSetpoints() @@ -171,4 +189,13 @@ void FlightTaskManualPosition::_updateSetpoints() _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); } } + + static uint32_t last_2s,delt_time; + uint32_t nowtime = hrt_absolute_time(); + if( (nowtime - last_2s) > 2_s) + { + mavlink_log_info(&_mavlink_log_pub, "_updateSetpoints(%d) : %.2f,%.2f\n",(nowtime - delt_time),(double)_position_setpoint(0),(double)_position_setpoint(1)); + last_2s = nowtime; + } + delt_time = hrt_absolute_time(); }