增加一些测试消息打印

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -66,6 +66,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/distance_sensor.h>
 
+#include <systemlib/mavlink_log.h>
+
 using namespace time_literals;
 
 /* Configuration Constants */
@@ -177,6 +179,8 @@ private:
 		(ParamInt<px4::params::SENS_TF_10_ROT>) _p_sensor10_rot,
 		(ParamInt<px4::params::SENS_TF_11_ROT>) _p_sensor11_rot
 	);
+	orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
+
 };
 
 TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
@@ -280,6 +284,9 @@ TFMINI_I2C::collect()
 	report.timestamp        = hrt_absolute_time();
 	report.type             = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 	report.variance         = 0.0f;
+	report.h_fov = math::radians(1.15f);	// 50.0f, 0.0488692f
+	report.v_fov = math::radians(1.15f);
+
 
 	int instance_id;
 	orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id);
@@ -293,6 +300,16 @@ TFMINI_I2C::collect()
 	}
 
 	perf_end(_sample_perf);
+
+	// 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, "dist(%d) : %.2f,%.2f\n",(nowtime-delt_time),(double)distance_m,(double)report.h_fov);
+	// 	last_2s = nowtime;
+	// }
+	// delt_time = hrt_absolute_time();
+
 	return PX4_OK;
 }
 

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -529,6 +529,16 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 	_constraints_pub.publish(constraints);
 
 	original_setpoint = new_setpoint;
+
+	static uint32_t last_2s,delt_time;
+	uint32_t nowtime = getTime();
+	if( (nowtime - last_2s) > 2_s)
+	{
+		printf( "Obstacle step(%d): %.2f,%.2f\n",delt_time,(double)original_setpoint(0),(double)original_setpoint(1));
+		last_2s = nowtime;
+	}
+	delt_time = getTime();
+
 }
 
 void CollisionPrevention::_publishVehicleCmdDoLoiter()

src/modules/commander/Commander.cpp

@@ -728,15 +728,20 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
 					main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
+					mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d\n",custom_main_mode,main_ret);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
 					main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state);
+					mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d\n",custom_main_mode,main_ret);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
 					reset_posvel_validity();
 					main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
+					mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d,lo:%d,go:%d\n",custom_main_mode,main_ret,_status_flags.condition_local_altitude_valid,_status_flags.condition_global_position_valid);
+					main_ret = TRANSITION_CHANGED;
+
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */

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

@@ -41,6 +41,8 @@
 #include <ecl/geo/geo.h>
 #include <drivers/drv_hrt.h>
 
+#include <systemlib/mavlink_log.h>
+
 using namespace matrix;
 
 FlightTaskManualAltitude::FlightTaskManualAltitude() :
@@ -376,14 +378,15 @@ bool FlightTaskManualAltitude::update()
 	_scaleSticks();
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
-
+	static uint32_t last_2s,delt_time;
-	static uint64_t delay_1s = hrt_absolute_time();
+	uint32_t nowtime = hrt_absolute_time();
-	static uint64_t delt_time;
+	if( (nowtime - last_2s) > 2 * 1000000ULL)
-	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
 	{
-		printf("update,delt:%dms \n",(hrt_absolute_time() - delt_time)/(1 * 1000ULL) );
+		mavlink_log_info(&_mavlink_log_pub, "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U);
-        delay_1s = hrt_absolute_time();
+		printf( "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U );
+		last_2s = nowtime;
 	}
 	delt_time = hrt_absolute_time();
+
 	return ret;
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -151,4 +151,5 @@ private:
 	float _dist_to_ground_lock = NAN;
 
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
+	orb_advert_t		_mavlink_log_pub{nullptr};
 };

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

@@ -39,6 +39,7 @@
 #include <mathlib/mathlib.h>
 #include <float.h>
 
+#include <drivers/drv_hrt.h>
 using namespace matrix;
 
 FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
@@ -114,6 +115,16 @@ void FlightTaskManualPosition::_scaleSticks()
 	}
 
 	_velocity_setpoint.xy() = vel_sp_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, "scaleSticks(%d) : %.2f,%.2f\n",delt_time,(double)vel_sp_xy(0),(double)vel_sp_xy(1));
+		last_2s = nowtime;
+	}
+	delt_time = hrt_absolute_time();
 }
 
 void FlightTaskManualPosition::_updateXYlock()

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -76,4 +76,6 @@ private:
 		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
 	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
+	orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
+
 };

src/modules/mavlink/streams/DISTANCE_SENSOR.hpp

@@ -94,6 +94,8 @@ private:
 				msg.min_distance     = dist_sensor.min_distance * 1e2f;     // m to cm
 				msg.orientation      = dist_sensor.orientation;
 				msg.covariance       = dist_sensor.variance * 1e4f;         // m^2 to cm^2
+				msg.horizontal_fov       = dist_sensor.h_fov;         // m^2 to cm^2
+				msg.vertical_fov       = dist_sensor.v_fov;         // m^2 to cm^2
 
 				mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg);
 

v5_build.sh

@@ -1,6 +1,6 @@
 date -R
 starttime=`date +'%Y-%m-%d %H:%M:%S'`
-make px4_fmu-v5_default
+make px4_fmu-v5_default upload
 endtime=`date +'%Y-%m-%d %H:%M:%S'`
 date -R
 start_seconds=$(date --date="$starttime" +%s);