Browse Source

增加一些测试打印消息

tf-1.12-print_test
那个Zeng 3 years ago
parent
commit
a46a1fe656
  1. 28
      src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
  2. 29
      src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

28
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@ -107,6 +107,16 @@ void FlightTaskManualAltitude::_scaleSticks() @@ -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() @@ -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;
}

29
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@ -72,6 +72,15 @@ bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s @@ -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() @@ -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() @@ -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() @@ -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();
}

Loading…
Cancel
Save