Finish MSP_OSD distanceToHome message

	🏠5000

Author: Li.Tianming

src/drivers/osd/msp_osd/msp_defines.h

@@ -473,6 +473,16 @@ struct msp_comp_gps_t {
 	uint8_t  heartbeat;       // toggles 0 and 1 for each change
 } __attribute__((packed));
 
+struct msp_rendor_distanceToHome_t{
+	uint8_t subCommand = 0x03; // 0x03 subcommand write string. fixed
+	uint8_t screenYPosition;
+	uint8_t screenXPosition;
+	uint8_t iconAttrs = 0x00; //
+	uint8_t iconIndex = 0x71; //distanceToHome icon
+
+	char str[6]; // 65536
+} __attribute__((packed));
+
 
 // values for msp_nav_status_t.mode
 #define MSP_NAV_STATUS_MODE_NONE   0

src/drivers/osd/msp_osd/msp_osd.cpp

@@ -402,18 +402,23 @@ void MspOsd::Run()
 		// update heartbeat
 		// _heartbeat = !_heartbeat;
 
-		// home_position_s home_position{};
-		// _home_position_sub.copy(&home_position);
+		home_position_s home_position{};
+		_home_position_sub.copy(&home_position);
 
-		// vehicle_global_position_s vehicle_global_position{};
-		// _vehicle_global_position_sub.copy(&vehicle_global_position);
+		vehicle_global_position_s vehicle_global_position{};
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
 
 		// // construct and send message
 		// const auto msg = msp_osd::construct_COMP_GPS(
 		// 			 home_position,
 		// 			 vehicle_global_position,
 		// 			 _heartbeat);
 		// this->Send(MSP_COMP_GPS, &msg);
+
+		 const auto msg =  msp_osd::construct_rendor_distanceToHome(home_position, vehicle_global_position);
+
+		this->Send(MSP_CMD_DISPLAYPORT, &msg, sizeof(msp_rendor_distanceToHome_t));
+
 	}
 
 	// MSP_ATTITUDE

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -411,6 +411,37 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position,
 	return comp_gps;
 }
 
+msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_s &home_position,
+				  const vehicle_global_position_s &vehicle_global_position)
+{
+	msp_rendor_distanceToHome_t distance;
+
+	distance.screenYPosition = 0x08; //
+	distance.screenXPosition = 0x02; //
+
+  int16_t dist_i = 0;
+
+	if (home_position.valid_hpos
+	    && home_position.valid_lpos
+	    && (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) {
+
+		float distance_to_home = get_distance_to_next_waypoint(vehicle_global_position.lat,
+					 vehicle_global_position.lon,
+					 home_position.lat, home_position.lon);
+
+		dist_i = (int16_t)distance_to_home; // meters
+
+	} else {
+		dist_i = 0; // meters
+	}
+
+  char num_str[7];
+  sprintf(num_str, "%d", dist_i); // 65536
+  strncpy(&distance.str[0], num_str, 6);
+
+	return distance;
+}
+
 msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude)
 {
 	// initialize results

src/drivers/osd/msp_osd/uorb_to_msp.hpp

@@ -120,6 +120,9 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 msp_rendor_altitude_t construct_Rendor_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 				  const vehicle_local_position_s &vehicle_local_position);
 
+msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_s &home_position,
+				  const vehicle_global_position_s &vehicle_global_position);
+
 // construct an MSP_ESC_SENSOR_DATA struct
 msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA();