이번 코드리뷰에서는 mavros/global_position 의 토픽들이 어떻게 만들어지는지, globalraw/fix 토픽의 Hz 가 다른 이유와 EKF2_AID_MASK ( 혹은 EKF2_GPS_CTRL ) 파라미터의 영향에 따른 Yaw 계산 방식을 살펴보고 compass_hdg 토픽은 어떻게 계산되는지 알아보고자 한다.

mavros/global_position

mavros 위키 에 있는 ~/raw/fix, ~/global 그리고 ~/compass_hdg 토픽이 어떤 식으로 Publish 되는지 알아보자.

우선 mavros 에서 이를 publish 하는 코드는 global_position.cpp 에 있다.

// 생략
	// gps data
	raw_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("raw/fix", 10);
	raw_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("raw/gps_vel", 10);
	raw_sat_pub = gp_nh.advertise<std_msgs::UInt32>("raw/satellites", 10);
 
	// fused global position
	gp_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("global", 10);
	gp_odom_pub = gp_nh.advertise<nav_msgs::Odometry>("local", 10);
	gp_rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
	gp_hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
}
 
Subscriptions get_subscriptions() override
{
	return {
		make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
		// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
		make_handler(&GlobalPositionPlugin::handle_global_position_int),
		make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
		make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)
	};
}

기본 Plugin 에 override 방식으로 코드 구조가 이루어져 있고, 각 정의된 토픽 Publisher 들은 아래 get_subscriptions()handle~ 함수들 안에 있다.

각 함수는 GPS_RAW_INT 와 GLOBAL_POSITION_INT 메세지(mavlink::common::msg)를 받아와 rosmsg 의 데이터를 구성한다. 이 메세지 타입들은 MAVLink 에 정의되어 있는 것으로 Messages 를 보면 잘 나타나 있고 코드로는 mavlink/c_library_v2 를 참조하면 된다.

아래에서 다루지만 global_position/global 의 LLA 정보는 raw/fix 처럼 그대로 받아오는 것이 아니라 EKF2 를 통해 얻은 VehicleGlobalPosition 에서 받아와 fill_lla(gpos, fix) 로 구현되어 있다. 1

대신, status_service 나 covariance 부분은 void handle_gps_raw_int()m_uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible); 부분에서 raw/fix 데이터를 저장해두고 void handle_global_position_intauto raw_fix = m_uas->get_gps_fix(); 에서 이를 가져와 global_position/global 토픽의 내용을 채운다.

Topic Hz (~/global vs ~/raw/fix)

위에서 언급한 LLA 정보를 받아오는 uORB 메세지의 차이로 인해 두 토픽의 Hz 는 다르게 나타난다.

raw 한 LLA 정보는 GPS Driver 로부터 직접 얻게되고 global 토픽은 EKF2 를 거쳐 다른 센서와 보정한 정보를 가진다고 하였는데 이를 자세히 살펴보자.

PX4 module 중 mavlink/stream 에서 GPS_RAW_INT.hpp 와 GLOBAL_POSITION_INT.hpp 을 보면 어떤 uORB 메세지를 받아서 패킷을 보내는지 알 수 있다.

GPS_RAW_INT.hpp
bool send() override
{
	sensor_gps_s gps;
 
	if (_sensor_gps_sub.update(&gps)) {
		mavlink_gps_raw_int_t msg{};
 
		msg.time_usec = gps.timestamp;
		msg.fix_type = gps.fix_type;
		msg.lat = gps.lat;
		msg.lon = gps.lon;
		msg.alt = gps.alt;
		msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless)
		msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless)
 
		if (PX4_ISFINITE(gps.vel_m_s) && (fabsf(gps.vel_m_s) >= 0.f)) {
			msg.vel = gps.vel_m_s * 100.f; // cm/s
 
		} else {
			msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX
		}
 
		msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f;
		msg.satellites_visible = gps.satellites_used;
		msg.alt_ellipsoid = gps.alt_ellipsoid;
		msg.h_acc = gps.eph * 1e3f;              // position uncertainty in mm
		msg.v_acc = gps.epv * 1e3f;              // altitude uncertainty in mm
		msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm
 
		if (PX4_ISFINITE(gps.heading)) {
			if (fabsf(gps.heading) < FLT_EPSILON) {
				msg.yaw = 36000; // Use 36000 for north.
 
			} else {
				msg.yaw = math::degrees(matrix::wrap_2pi(gps.heading)) * 100.0f; // centidegrees
			}
 
			if (PX4_ISFINITE(gps.heading_accuracy)) {
				msg.hdg_acc = math::degrees(gps.heading_accuracy) * 1e5f; // Heading / track uncertainty in degE5
			}
		}
 
		mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
 
		return true;
	}
 
	return false;
}

보다시피 GPS_RAW_INT 는 sensor_gps_s 를 subscribe 해서 메세지를 할당한다. 시뮬레이션을 제외하고 유일한 uORB::PublicationMulti<sensor_gps_s> 는 src/driver 에 위치한다. 각 GPS 제품에 따라 다르게 파싱해서 받아와 publish 한다.

GLOBAL_POSITION_INT.hpp
uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _home_sub{ORB_ID(home_position)};
uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)};
 
bool send() override
{
	vehicle_global_position_s gpos;
	vehicle_local_position_s lpos;
 
	if (_gpos_sub.update(&gpos) && _lpos_sub.update(&lpos)) {
 
		mavlink_global_position_int_t msg{};
 
		if (lpos.z_valid && lpos.z_global) {
			msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f;
 
		} else {
			// fall back to baro altitude
			vehicle_air_data_s air_data{};
			_air_data_sub.copy(&air_data);
 
			if (air_data.timestamp > 0) {
				msg.alt = air_data.baro_alt_meter * 1000.0f;
			}
		}
 
		home_position_s home{};
		_home_sub.copy(&home);
 
		if ((home.timestamp > 0) && home.valid_alt) {
			if (lpos.z_valid) {
				msg.relative_alt = -(lpos.z - home.z) * 1000.0f;
 
			} else {
				msg.relative_alt = msg.alt - (home.alt * 1000.0f);
			}
 
		} else {
			if (lpos.z_valid) {
				msg.relative_alt = -lpos.z * 1000.0f;
			}
		}
 
		msg.time_boot_ms = gpos.timestamp / 1000;
		msg.lat = gpos.lat * 1e7;
		msg.lon = gpos.lon * 1e7;
 
		msg.vx = lpos.vx * 100.0f;
		msg.vy = lpos.vy * 100.0f;
		msg.vz = lpos.vz * 100.0f;
 
		msg.hdg = math::degrees(matrix::wrap_2pi(lpos.heading)) * 100.0f;
 
		mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
 
		return true;
	}
 
	return false;
}

GLOBAL_POSITION_INT 은 VehicleGlobalPosition 의 위도 경도와 VehicleLocalPosition 의 높이, heading 정보로 메세지를 담아 보낸다. 이 둘은 각각 EKF2 를 거쳐 fusion 한 정보에 해당한다.

그래서 raw/fix 토픽은 GPS 에서 낼 수 있는 Max Hz 로 publish 된다. 이와 관련한 이슈2 가 있어서 driver 코드를 확인해보니 아래와 같이 기존에 설정해놓은 baudrate 를 사용하거나 사용가능한 최대 baudrate 를 사용하도록 되어 있었다.

추가적으로 소스코드 에서 configure()recieve() 함수를 보면 된다. 사용하는 GPS 가 NMEA 형식을 사용하여 nmea.cpp 로 살펴보았다.

https://github.com/PX4/PX4-GPSDrivers/blob/main/src/nmea.cpp#L1065
int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config)
{
	_output_mode = config.output_mode;
 
	if (_output_mode != OutputMode::GPS) {
		NMEA_WARN("RTCM output have to be configured manually");
	}
 
	// If a baudrate is defined, we test this first
	if (baudrate > 0) {
		setBaudrate(baudrate);
		decodeInit();
		int ret = receive(400);
		gps_usleep(2000);
 
		// If a valid POS message is received we have GPS
		if (_POS_received || ret > 0) {
			return 0;
		}
	}
 
	// If we haven't found the GPS with the defined baudrate, we try other rates
	const unsigned baudrates_to_try[] = {9600, 19200, 38400, 57600, 115200, 230400};
	unsigned test_baudrate;
 
	for (unsigned int baud_i = 0; !_POS_received
	     && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
 
		test_baudrate = baudrates_to_try[baud_i];
		setBaudrate(test_baudrate);
 
		NMEA_DEBUG("baudrate set to %i", test_baudrate);
 
		decodeInit();
		int ret = receive(400);
		gps_usleep(2000);
 
		// If a valid POS message is received we have GPS
		if (_POS_received || ret > 0) {
			return 0;
		}
	}
 
	// If nothing is found we leave the specified or default
	if (baudrate > 0) {
		return setBaudrate(baudrate);
	}
 
	return setBaudrate(NMEA_DEFAULT_BAUDRATE);
}

Unicore UM982 의 경우 기본적으로 230400 baudrate 를 사용하여 5Hz 의 주기로 정보를 수신하는 것으로 보인다. 링크

이제 global 토픽의 Hz 에 대해 알아보자. Inspector 로 확인해본 결과에 따르면 GLOBAL_POSITION_INT 메세지는 50Hz 의 주기로 송수신된다고 한다.3

하지만 이는 EKF2_*_DELAY 와 filtering 관련 파라미터에 의해 바뀌는 것으로 보인다.

Reference

위 GLOBAL_POSITION_INT 일부 코드에서 위도 경도 정보는 VehicleGlobalPosition 에서 받아온다고 하였는데 이를 Publish 하는 부분은 modules/ekf2 중 EKF2.cpp 에 위치한다.

EKF2.cpp
void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
{
	if (_ekf.global_position_is_valid()) {
		const Vector3f position{_ekf.getPosition()};
 
		// generate and publish global position data
		vehicle_global_position_s global_pos;
		global_pos.timestamp_sample = timestamp;
 
		// Position of local NED origin in GPS / WGS84 frame
		_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
 
		float delta_xy[2];
		_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
 
		global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
		global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
 
		// global altitude has opposite sign of local down position
		float delta_z;
		uint8_t z_reset_counter;
		_ekf.get_posD_reset(&delta_z, &z_reset_counter);
		global_pos.delta_alt = -delta_z;
 
		_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
 
		if (_ekf.isTerrainEstimateValid()) {
			// Terrain altitude in m, WGS84
			global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
			global_pos.terrain_alt_valid = true;
 
		} else {
			global_pos.terrain_alt = NAN;
			global_pos.terrain_alt_valid = false;
		}
 
		global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
					    || _ekf.control_status_flags().wind_dead_reckoning;
		global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
		_global_position_pub.publish(global_pos);
	}
}

보다시피 _ekf.getPosition() 으로 받아온 xy 값을 다시 reprojection 하여 lat, lon 정보를 얻어낸다.

// get the position of the body frame origin in local earth frame
Vector3f getPosition() const
{
	// rotate the position of the IMU relative to the boy origin into earth frame
	const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
	// subtract from the EKF position (which is at the IMU) to get position at the body origin
	return _output_new.pos - pos_offset_earth;
}

디테일한 파트는 생략하겠지만, 비교적 낮은 Hz 로 얻는 GPS raw data 로 기준 위도, 경도를 만들고 여러 센서로 fusion 하여 보다 정확한 위치 추정을 하고 있다.

문제는 이 PublishGlobalPosition 함수가 호출되는 EKF2::Run() 함수가 어디서 어떤 주기로 발생하는지 확인하기 어려웠다.

다만 _output_new 가 서로 다른 time horiozn 의 센서 정보를 fusion 하는 ekf2/EKF/ekf.cpp 에서 update 된다는 것만 알 수 있었다. (v1.14 부터는 코드구조가 바뀌어 output_predictor.cpp 에 있다.)

// use trapezoidal integration to calculate the INS position states
// do the same for vertical state used by alternative correction algorithm
const Vector3f delta_pos_NED = (_output_new.vel + vel_last) * (imu.delta_vel_dt * 0.5f);
_output_new.pos += delta_pos_NED;

mavros plugin

다시 처음 global_position.cpp 로 돌아가보면 make_handler 에서 각 handler~ 함수를 넣어주었다. 이 make_handlermavros_plugin.h 에 정의되어 있다. 소스코드

대부분의 PX4 에서 mavros 로 만들어내는 토픽들은 이러한 형태를 가지고 있다.

mavros_plugin.h
//! generic message handler callback
using HandlerCb = mavconn::MAVConnInterface::ReceivedCb;
//! Tuple: MSG ID, MSG NAME, message type into hash_code, message handler callback
using HandlerInfo = std::tuple<mavlink::msgid_t, const char*, size_t, HandlerCb>;
 
// ...생략
 
/**
 * Make subscription to message with automatic decoding.
 *
 * @param[in] fn  pointer to member function (handler)
 */
template<class _C, class _T>
HandlerInfo make_handler(void (_C::*fn)(const mavlink::mavlink_message_t*, _T &)) {
	auto bfn = std::bind(fn, static_cast<_C*>(this), std::placeholders::_1, std::placeholders::_2);
	const auto id = _T::MSG_ID;
	const auto name = _T::NAME;
	const auto type_hash_ = typeid(_T).hash_code();
 
	return HandlerInfo{
			   id, name, type_hash_,
			   [bfn](const mavlink::mavlink_message_t *msg, const mavconn::Framing framing) {
				   if (framing != mavconn::Framing::ok)
					   return;
 
				   mavlink::MsgMap map(msg);
				   _T obj;
				   obj.deserialize(map);
 
				   bfn(msg, obj);
			   }
	};
}

즉 mavlink 의 메세지를 deserialize 해서 정보를 받아오는 것으로 보이고 이 함수는 mavlink 빌드 시 생겨나는 함수에 정의되어 있다. GPS_RAW_INT 메세지를 예시로 들면 아래와 같고, 아마도 위에 mavlink_msg_gps_raw_int_send_struct 에서 정보를 담아 보낸 것을 handler 에서 호출될 때 꺼내서 사용하는 방식인 것 같은데, 정확한 문법을 파악하지는 못하였다. 대략적으로 uORB -> mavlink packet -> mavros 이런 구조가 아닐까…

EKF2 Heading

마지막으로 Heading 계산 방법과 compass_hdg 토픽이 어떤 정보를 담고 있는지 알아보고자 한다.

PX4 공식문서에 따르면 main 과 v1.13 버전에서 사용하는 파라미터가 상이하다. main, v1.13

깃헙 PR 을 살펴보아도 v1.14.0-beta2 부터 적용되었고, GPS Heading 을 지원하는 Unicore UM982 driver 도 이때 추가되었다.4

NAMEDESCRIPTION
EKF2_AID_MASK (INT32) (v1.13)Integer bitmask controlling data fusion and aiding methods

Comment: Set bits in the following positions to enable: 0 : Set to true to use GPS data if available 1 : Set to true to use optical flow data if available 2 : Set to true to inhibit IMU delta velocity bias estimation 3 : Set to true to enable vision position fusion 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true. 5 : Set to true to enable multi-rotor drag specific force fusion 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.

Bitmask:

- 0: use GPS
- 1: use optical flow
- 2: inhibit IMU bias estimation
- 3: vision position fusion
- 4: vision yaw fusion
- 5: multi-rotor drag fusion
- 6: rotate external vision
- 7: GPS yaw fusion

Reboot required: true
EKF2_GPS_CTRL (INT32) (v1.14+)GNSS sensor aiding

Comment: Set bits in the following positions to enable: 0 : Longitude and latitude fusion 1 : Altitude fusion 2 : 3D velocity fusion 3 : Dual antenna heading fusion

Bitmask:

- 0: Lon/lat
- 1: Altitude
- 2: 3D velocity
- 3: Dual antenna heading

global_position.cpp 에서 compass_hdg 토픽 정보를 받는 부분을 알기 위해 GLOBAL_POSITION_INT.hpp 코드를 다시 살펴보면 msg.hdg = math::degrees(matrix::wrap_2pi(lpos.heading)) * 100.0f; 라인에서 heading 정보를 넣어주므로, 이는 VehicleLocalPosition 에서 온 정보임을 알 수 있다.

VehicleLocalPosition 을 Publish 하는 부분은 EKF2.cpp 에 EKF2::PublishLocalPosition() 에 구현되어 있다.

v1.13

아래에 PublishLocalPosition() 부터 heading 이 어떤 흐름으로 정보를 받아오는지 간략히 정리하였다.(v1.13 기준)

// EKF2.cpp PublishLocalPosition()
lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
 
// estimator_interface.h
const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
 
// EKF::calculateOutputStates
	const Quatf dq(AxisAnglef{delta_angle});
	
	// rotate the previous INS quaternion by the delta quaternions
	_output_new.quat_nominal = _output_new.quat_nominal * dq;
	
	// the quaternions must always be normalised after modification
	_output_new.quat_nominal.normalize();
	
	// calculate the rotation matrix from body to earth frame
	_R_to_earth_now = Dcmf(_output_new.quat_nominal);

문제는 위의 EKF2_AID_MASK 에 따라서 어떻게 yaw 를 선택해서 _output_new 혹은 delta_angle 등의 변수들이 계산되는지는 연결짓지 못했다. v1.14 이후에는 CONFIG_EKF2_GNSS_YAW 조건문으로 따로 나뉘어져 있어 보기 조금 쉽고, v1.13 에서는 _ekf2_aid_mask 혹은 MASK_USE_GPSYAW 로 살펴보면 된다.

// Bit locations for fusion_mode
#define MASK_USE_GPS    (1<<0)		///< set to true to use GPS data
#define MASK_USE_OF     (1<<1)		///< set to true to use optical flow data
#define MASK_INHIBIT_ACC_BIAS (1<<2)	///< set to true to inhibit estimation of accelerometer delta velocity bias
#define MASK_USE_EVPOS	(1<<3)		///< set to true to use external vision position data
#define MASK_USE_EVYAW  (1<<4)		///< set to true to use external vision quaternion data for yaw
#define MASK_USE_DRAG  (1<<5)		///< set to true to use the multi-rotor drag model to estimate wind
#define MASK_ROTATE_EV  (1<<6)		///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
#define MASK_USE_GPSYAW  (1<<7)		///< set to true to use GPS yaw data if available
#define MASK_USE_EVVEL  (1<<8)		///< set to true to use external vision velocity data
EKF::controlGpsFusion()
void Ekf::controlGpsFusion()
{
	if (!(_params.fusion_mode & MASK_USE_GPS)) {
		stopGpsFusion();
		return;
	}

v1.13 에서는 MASK_USE_GPS 가 true 이면 fusion 을 수행하는 것으로 보이고, Ekf::update()controlFusionModes() 에서 각 센서들의 정보를 처리해 이를 fusion 할 지 판단하고 사용한다.

EKF::update()
bool Ekf::update()
{
	bool updated = false;
 
	if (!_filter_initialised) {
		_filter_initialised = initialiseFilter();
 
		if (!_filter_initialised) {
			return false;
		}
	}
 
	// Only run the filter if IMU data in the buffer has been updated
	if (_imu_updated) {
		// perform state and covariance prediction for the main filter
		predictState();
		predictCovariance();
 
		// control fusion of observation data
		controlFusionModes();
 
		// run a separate filter for terrain estimation
		runTerrainEstimator();
 
		updated = true;
	}
 
	// the output observer always runs
	// Use full rate IMU data at the current time horizon
	calculateOutputStates(_newest_high_rate_imu_sample);
 
	return updated;
}

보다 자세한 내용은 control.cpp, gps_control.cppgps_yaw_fusion.cpp 을 살펴보면 된다.

Question

  • fuseGpsYaw() 에서 계산한 결과가 어떻게 calculateOutputStates() 에서 사용되는지?

v1.14+

// EKF2.cpp
#if defined(CONFIG_EKF2_GNSS_YAW)
 
	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
		_estimator_aid_src_gnss_yaw_pub.advertise();
	}
 
#endif // CONFIG_EKF2_GNSS_YAW
 
// ekf.h
#if defined(CONFIG_EKF2_GNSS_YAW)
		if (_control_status.flags.gps_yaw) {
			heading_innov = _aid_src_gnss_yaw.innovation;
			return;
		}
#endif // CONFIG_EKF2_GNSS_YAW
 
// gps_control.cpp
#if defined(CONFIG_EKF2_GNSS_YAW)
		controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
#endif // CONFIG_EKF2_GNSS_YAW

v1.14 이후부터는 EKF2_GNSS_CTRL 파라미터를 사용하며 내부 코드에서는 CONFIG_EKF2_GNSS_YAW 에 따라 제어되는 것을 알 수 있다. 또 다른 점 중 한가지는 EstimatorAidSource1d.msg 가 uORB 메시지 타입으로 추가되어서 fusion 한 정보를 사용하고 있었다.

_heading_innov 혹은 gnss_yaw.innovation 으로 보아 해당 내용이 fusion 할 때 같이 들어가는 것으로 보이는데, 여전히 calculateOutputStates() 와 연결짓기 어려웠다.

Question

  • _aid_src_gnss_yaw 가 어디에서 Subscribe 되는지? 혹은

Footnotes

  1. https://discuss.px4.io/t/what-is-the-difference-between-gps-and-global-position-and-local-position/34908

  2. https://discuss.px4.io/t/gps-refresh-rate/16230

  3. https://github.com/mavlink/mavlink/issues/993

  4. https://github.com/PX4/PX4-Autopilot/pull/21214