增加一些测试打印消息

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;
+	// static uint32_t last_2s,delt_time;
-	uint32_t nowtime = hrt_absolute_time();
+	// uint32_t nowtime = hrt_absolute_time();
-	if( (nowtime - last_2s) > 2 * 1000000ULL)
+	// if( (nowtime - last_2s) > 2 * 1000000ULL)
-	{
+	// {
-		mavlink_log_info(&_mavlink_log_pub, "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U);
+	// 	mavlink_log_info(&_mavlink_log_pub, "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U);
-		printf( "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U );
+	// 	printf( "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U );
-		last_2s = nowtime;
+	// 	last_2s = nowtime;
-	}
+	// }
-	delt_time = hrt_absolute_time();
+	// delt_time = hrt_absolute_time();
 
 	return ret;
 }

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();
 }