Wrong velocity and cog in the fake_gps plugin

[shupx]

mavros/mavros_extras/src/plugins/fake_gps.cpp

Lines 286 to 353 in c655e63

	void send_fake_gps(const rclcpp::Time & stamp, const Eigen::Vector3d & ecef_offset)
	{
	auto now_ = node->now();
	// Throttle incoming messages
	if ((now_ - last_pos_time).to_chrono<std::chrono::nanoseconds>() < gps_rate_period) {
	return;
	}
	last_pos_time = now_;
	Eigen::Vector3d geodetic;
	Eigen::Vector3d current_ecef(ecef_origin.x() + ecef_offset.x(),
	ecef_origin.y() + ecef_offset.y(),
	ecef_origin.z() + ecef_offset.z());
	try {
	earth.Reverse(
	current_ecef.x(), current_ecef.y(), current_ecef.z(),
	geodetic.x(), geodetic.y(), geodetic.z());
	} catch (const std::exception & e) {
	RCLCPP_INFO_STREAM(get_logger(), "FGPS: Caught exception: " << e.what());
	}
	Eigen::Vector3d vel = (old_ecef - current_ecef) / (stamp.seconds() - old_stamp); // [m/s]
	// store old values
	old_stamp = stamp.seconds();
	old_ecef = current_ecef;
	if (use_hil_gps) {
	/**
	* @note: <a href="https://mavlink.io/en/messages/common.html#HIL_GPS">HIL_GPS MAVLink message</a>
	* is supported by both Ardupilot and PX4 Firmware.
	* But on PX4 Firmware are only acceped out of HIL mode
	* if use_hil_gps flag is set (param MAV_USEHILGPS = 1).
	*/
	mavlink::common::msg::HIL_GPS hil_gps {};
	vel *= 1e2; // [cm/s]
	// compute course over ground
	double cog;
	if (vel.x() == 0 && vel.y() == 0) {
	cog = 0;
	} else if (vel.x() >= 0 && vel.y() < 0) {
	cog = M_PI * 5 / 2 - atan2(vel.x(), vel.y());
	} else {
	cog = M_PI / 2 - atan2(vel.x(), vel.y());
	}
	// Fill in and send message
	hil_gps.time_usec = get_time_usec(stamp); // [useconds]
	hil_gps.lat = geodetic.x() * 1e7; // [degrees * 1e7]
	hil_gps.lon = geodetic.y() * 1e7; // [degrees * 1e7]
	hil_gps.alt = uas->data.egm96_5->ConvertHeight(
	geodetic.x(), geodetic.y(), geodetic.z(),
	GeographicLib::Geoid::ELLIPSOIDTOGEOID) * 1e3; // [meters * 1e3]
	hil_gps.vel = vel.block<2, 1>(0, 0).norm(); // [cm/s]
	hil_gps.vn = vel.x(); // [cm/s]
	hil_gps.ve = vel.y(); // [cm/s]
	hil_gps.vd = vel.z(); // [cm/s]
	hil_gps.cog = cog * 1e2; // [degrees * 1e2]
	hil_gps.eph = eph * 1e2; // [cm]
	hil_gps.epv = epv * 1e2; // [cm]
	hil_gps.fix_type = utils::enum_value(fix_type);
	hil_gps.satellites_visible = satellites_visible;
	uas->send_message(hil_gps);

The calculation method for hil_gps vn,ve,vd in the fake_gps module is incorrect. Here, vel is the velocity obtained by the difference between old_ecef and current_ecef, meaning that vel is located in the ECEF coordinate system. However, the actual vn,ve,vd should be values ​​in the NED coordinate system. Therefore, the calculation of vel here is wrong.

Another issue is that the unit of hil_gps.cog should be degree * 1e2, but in this module, cog is calculated in radians.

These two problems lead to the wrong HIL GPS mavlink streams and fail to provide the drone a accurate fake gps location. I have tested on real drones and find these two issues.

The same problem lies in ROS1 branch too.