refactor(sih): some suggestions from trying to review

Honestly I don't understand why there needs to be another reference. I'll need to continue reviewing later since it's not trivial.
fix: SIH issue with jumps in local and global position
2026-06-20 23:20:34 +08:00 · 2026-03-13 14:54:06 +01:00 · 2026-03-13 13:36:12 +01:00 · 2026-03-12 20:16:05 -07:00
11 changed files with 64 additions and 138 deletions
@@ -6,32 +6,42 @@ cp **/**/*.elf artifacts/ 2>/dev/null || true
 for build_dir_path in build/*/ ; do
  build_dir_path=${build_dir_path::${#build_dir_path}-1}
  build_dir=${build_dir_path#*/}
-  mkdir artifacts/$build_dir
+  mkdir -p artifacts/$build_dir
  find artifacts/ -maxdepth 1 -type f -name "*$build_dir*"
-  # Airframe
-  cp $build_dir_path/airframes.xml artifacts/$build_dir/
+  # Airframe (NuttX: build root, SITL: docs/ subdirectory)
+  airframes_src=""
+  if [ -f "$build_dir_path/airframes.xml" ]; then
+    airframes_src="$build_dir_path/airframes.xml"
+  elif [ -f "$build_dir_path/docs/airframes.xml" ]; then
+    airframes_src="$build_dir_path/docs/airframes.xml"
+  fi
+  if [ -n "$airframes_src" ]; then
+    cp "$airframes_src" "artifacts/$build_dir/"
+  fi
  # Parameters
-  cp $build_dir_path/parameters.xml artifacts/$build_dir/
-  cp $build_dir_path/parameters.json artifacts/$build_dir/
-  cp $build_dir_path/parameters.json.xz artifacts/$build_dir/
+  cp $build_dir_path/parameters.xml artifacts/$build_dir/ 2>/dev/null || true
+  cp $build_dir_path/parameters.json artifacts/$build_dir/ 2>/dev/null || true
+  cp $build_dir_path/parameters.json.xz artifacts/$build_dir/ 2>/dev/null || true
  # Actuators
-  cp $build_dir_path/actuators.json artifacts/$build_dir/
-  cp $build_dir_path/actuators.json.xz artifacts/$build_dir/
+  cp $build_dir_path/actuators.json artifacts/$build_dir/ 2>/dev/null || true
+  cp $build_dir_path/actuators.json.xz artifacts/$build_dir/ 2>/dev/null || true
  # Events
-  cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/
-  # ROS 2 msgs
-  cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/
-  # Module Docs
+  mkdir -p artifacts/$build_dir/events/
+  cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/events/ 2>/dev/null || true
  ls -la artifacts/$build_dir
  echo "----------"
 done

 if [ -d artifacts/px4_sitl_default ]; then
-  # general metadata
-  mkdir artifacts/_general/
-  cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+  # general metadata (used by Flight Review and other downstream consumers)
+  mkdir -p artifacts/_general/
  # Airframe
-  cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+  if [ -f artifacts/px4_sitl_default/airframes.xml ]; then
+    cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+  else
+    echo "Error: expected 'artifacts/px4_sitl_default/airframes.xml' not found." >&2
+    exit 1
+  fi
  # Parameters
  cp artifacts/px4_sitl_default/parameters.xml artifacts/_general/
  cp artifacts/px4_sitl_default/parameters.json artifacts/_general/
@@ -40,9 +50,11 @@ if [ -d artifacts/px4_sitl_default ]; then
  cp artifacts/px4_sitl_default/actuators.json artifacts/_general/
  cp artifacts/px4_sitl_default/actuators.json.xz artifacts/_general/
  # Events
-  cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
-  # ROS 2 msgs
-  cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
-  # Module Docs
+  if [ -f artifacts/px4_sitl_default/events/all_events.json.xz ]; then
+    cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
+  else
+    echo "Error: expected 'artifacts/px4_sitl_default/events/all_events.json.xz' not found." >&2
+    exit 1
+  fi
  ls -la artifacts/_general/
 fi
@@ -72,6 +72,15 @@ void Sih::run()
 	_px4_accel.set_temperature(T1_C);
 	_px4_gyro.set_temperature(T1_C);

+	// Initialize position, velocity and attitude
+	_lla = LatLonAlt(_sih_lat0.get(), _sih_lon0.get(), _sih_h0.get());
+	_p_E = _p_E_ref = _lla.toEcef();
+	_lpos_ref.initReference(_lla.latitude_deg(), _lla.longitude_deg()); // Initialize MapProjection reference
+	_lpos_ref_alt = _lla.altitude();
+	_R_E2N_ref = computeRotEcefToNed(_lla); // Reference frame rotation (fixed, used for local position computation)
+	_R_N2E = _R_E2N_ref.transpose();
+	_q_E = Quatf(_R_N2E) * _q;
+
 	parameters_updated();

 	const hrt_abstime task_start = hrt_absolute_time();
@@ -246,27 +255,6 @@ void Sih::parameters_updated()
 	_KDV = _sih_kdv.get();
 	_KDW = _sih_kdw.get();

-	if (!_lpos_ref.isInitialized()
-	    || (fabsf(static_cast<float>(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON)
-	    || (fabsf(static_cast<float>(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON)
-	    || (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) {
-		_lpos_ref.initReference(static_cast<double>(_sih_lat0.get()), static_cast<double>(_sih_lon0.get()));
-		_lpos_ref_alt = _sih_h0.get();
-
-		// Reset earth position, velocity and attitude
-		_lla.setLatitudeDeg(static_cast<double>(_sih_lat0.get()));
-		_lla.setLongitudeDeg(static_cast<double>(_sih_lon0.get()));
-		_lla.setAltitude(_lpos_ref_alt);
-		_p_E = _lla.toEcef();
-
-		const Dcmf R_E2N = computeRotEcefToNed(_lla);
-		_R_N2E = R_E2N.transpose();
-		_v_E = _R_N2E * _v_N;
-
-		_q_E = Quatf(_R_N2E) * _q;
-		_q_E.normalize();
-	}
-
 	_MASS = _sih_mass.get();

 	_I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get()));
@@ -392,6 +380,7 @@ void Sih::generate_ts_aerodynamics()
 	// the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly)
 	Vector3f v_ts = _R_S2B.transpose() * v_B;
 	Vector3f w_ts = _R_S2B.transpose() * _w_B;
+	// NED frame: _lpos(2) above reference is negative
 	float altitude = _lpos_ref_alt - _lpos(2);

 	Vector3f Fa_ts{};
@@ -563,8 +552,10 @@ void Sih::equations_of_motion(const float dt)

 	ecefToNed();

-	_lpos_ref.project(_lla.latitude_deg(), _lla.longitude_deg(), _lpos(0), _lpos(1));
-	_lpos(2) = -(_lla.altitude() - _lpos_ref_alt);
+	// Compute local position directly from ECEF difference (avoids lat/lon precision issues)
+	// Use the fixed reference rotation to maintain a consistent local frame
+	const Vector3d dp_E = _p_E - _p_E_ref;
+	_lpos = _R_E2N_ref * Vector3f(dp_E);
 }

 void Sih::ecefToNed()
@@ -741,13 +732,18 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us)

 	{
 		// publish global position groundtruth
+		// Use MapProjection reproject to ensure consistency with local position
 		vehicle_global_position_s global_position{};
 		global_position.timestamp_sample = time_now_us;
-		global_position.lat = _lla.latitude_deg();
-		global_position.lon = _lla.longitude_deg();
-		global_position.alt = _lla.altitude();
+
+		double lat, lon;
+		_lpos_ref.reproject(_lpos(0), _lpos(1), lat, lon);
+		global_position.lat = lat;
+		global_position.lon = lon;
+		// NED convention: _lpos(2) is negative when above reference, so alt = ref - (-height) = ref + height
+		global_position.alt = static_cast<double>(_lpos_ref_alt) - static_cast<double>(_lpos(2));
 		global_position.alt_ellipsoid = global_position.alt;
-		global_position.terrain_alt = -_lpos(2);
+		global_position.terrain_alt = static_cast<double>(_lpos_ref_alt);
 		global_position.timestamp = hrt_absolute_time();
 		_global_position_ground_truth_pub.publish(global_position);
 	}
@@ -228,6 +228,8 @@ private:

 	LatLonAlt _lla{};
 	matrix::Vector3f _lpos{};  // position in a local tangent-plane frame [m]
+	matrix::Vector3d _p_E_ref{}; // ECEF reference position for local frame origin [m]
+	matrix::Dcmf _R_E2N_ref{}; // Rotation from ECEF to reference NED frame

 	float _u[NUM_ACTUATORS_MAX] {}; // thruster signals

@@ -242,6 +242,7 @@ PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
 * @unit deg
 * @min -90
 * @max  90
+ * @reboot_required true
 * @group Simulation In Hardware
 */
 PARAM_DEFINE_FLOAT(SIH_LOC_LAT0, 47.397742f);
@@ -257,6 +258,7 @@ PARAM_DEFINE_FLOAT(SIH_LOC_LAT0, 47.397742f);
 * @unit deg
 * @min -180
 * @max  180
+ * @reboot_required true
 * @group Simulation In Hardware
 */
 PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f);
@@ -278,6 +280,7 @@ PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f);
 * @max 8848.0
 * @decimal 2
 * @increment 0.01
+ * @reboot_required true
 * @group Simulation In Hardware
 */
 PARAM_DEFINE_FLOAT(SIH_LOC_H0, 489.4f);
@@ -118,7 +118,6 @@ else()
 		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
 			--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
 			--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
-			--whitelist-file ${CMAKE_CURRENT_SOURCE_DIR}/safety_whitelist.yaml
 			--template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
 		DEPENDS
 			${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
@@ -180,10 +180,8 @@ struct RcvTopicsPubs {
@[    end for]@

 	uint32_t num_payload_received{};
-	bool _allow_publishing{false};

 	bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace);
-	void allow_publishing(bool enabled) { _allow_publishing = enabled; }
 };

 static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id,
@@ -198,17 +196,10 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	case @(idx)+ (65535U / 32U) + 1: {
 			@(sub['simple_base_type'])_s data;

-@[        if sub['topic_simple'] in whitelist_topics]@
 			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
 				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
 				pubs->@(sub['topic_simple'])_pub.publish(data);
 			}
-@[        else]@
-			if (pubs->_allow_publishing && ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
-				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
-				pubs->@(sub['topic_simple'])_pub.publish(data);
-			}
-@[        end if]@
 		}
 		break;

@@ -217,10 +208,9 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 	case @(idx + len(subscriptions))+ (65535U / 32U) + 1: {
 			@(sub['simple_base_type'])_s data;

+			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
+				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
@[        if sub.get('route_field')]@
-@[            if sub['topic_simple'] in whitelist_topics]@
-			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
-				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
 				int instance = -1;

 				for (uint8_t i = 0; i < pubs->@(sub['topic_simple'])_demux.num_assigned; i++) {
@@ -238,42 +228,10 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t
 				if (instance >= 0) {
 					pubs->@(sub['topic_simple'])_pubs[instance].publish(data);
 				}
-			}
-@[            else]@
-			if (pubs->_allow_publishing && ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
-				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
-				int instance = -1;
-
-				for (uint8_t i = 0; i < pubs->@(sub['topic_simple'])_demux.num_assigned; i++) {
-					if (pubs->@(sub['topic_simple'])_demux.assigned_ids[i] == data.@(sub['route_field'])) {
-						instance = i;
-						break;
-					}
-				}
-
-				if (instance < 0 && pubs->@(sub['topic_simple'])_demux.num_assigned < @(sub['max_instances'])) {
-					instance = pubs->@(sub['topic_simple'])_demux.num_assigned++;
-					pubs->@(sub['topic_simple'])_demux.assigned_ids[instance] = data.@(sub['route_field']);
-				}
-
-				if (instance >= 0) {
-					pubs->@(sub['topic_simple'])_pubs[instance].publish(data);
-				}
-			}
-@[            end if]@
@[        else]@
-@[            if sub['topic_simple'] in whitelist_topics]@
-			if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
-				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
 				pubs->@(sub['topic_simple'])_pub.publish(data);
-			}
-@[            else]@
-			if (pubs->_allow_publishing && ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) {
-				//print_message(ORB_ID(@(sub['simple_base_type'])), data);
-				pubs->@(sub['topic_simple'])_pub.publish(data);
-			}
-@[            end if]@
@[        end if]@
+			}
 		}
 		break;

@@ -51,9 +51,6 @@ parser.add_argument("-t", "--template_file", dest='template_file', type=str,

 parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str,
                    help="Client output dir, by default using relative path 'src/modules/uxrce_dds_client'", default=None)
-parser.add_argument("-w", "--whitelist-file", dest='whitelist_file', type=str,
-                    help="Whitelist topics file path for topics that publish regardless of _allow_publishing flag",
-                    default=None)

 if len(sys.argv) <= 1:
    parser.print_usage()
@@ -141,15 +138,6 @@ merged_em_globals['subscriptions_multi'] = subs_multi

 merged_em_globals['type_includes'] = sorted(set(all_type_includes))

-# Load whitelist topics that should not be fail-safed
-whitelist_topics = set()
-if args.whitelist_file and os.path.exists(args.whitelist_file):
-    with open(args.whitelist_file, 'r') as f:
-        whitelist_data = yaml.safe_load(f)
-        if whitelist_data and '__whitelist' in whitelist_data:
-            whitelist_topics = set(whitelist_data['__whitelist'])
-
-merged_em_globals['whitelist_topics'] = whitelist_topics

 # run interpreter
 ofile = open(output_file, 'w')
@@ -153,14 +153,3 @@ parameters:
            category: System
            reboot_required: true
            default: 0
-
-        UXRCE_DDS_SAFE:
-            description:
-                short: Enables offboard safety protection
-                long: |
-                    If disable, allows offboard passthrough
-                    even in non-offboard modes.
-            type: boolean
-            category: System
-            reboot_required: true
-            default: 1
@@ -1,4 +0,0 @@
-module_name: uxrce_dds_client
-
-__whitelist:
-  - vehicle_command
@@ -374,8 +374,6 @@ bool UxrceddsClient::setupSession(uxrSession *session)
 	}

 	_connected = true;
-
-	_safe_dds_mode = _param_uxrce_dds_safe.get();
 	return true;
 }

@@ -653,16 +651,6 @@ void UxrceddsClient::run()

 			int bytes_available = 0;

-			// Update vehicle status to check for offboard mode
-			vehicle_status_s vehicle_status{};
-			_vehicle_status_sub.copy(&vehicle_status);
-			_offboard_mode_enabled = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD);
-
-			// Allow publish from DDS to uORB if:
-			// - _param_uxrce_dds_safe is false , regardless of offboard mode
-			// - _param_uxrce_dds_safe is true AND offboard mode is enabled
-			_pubs->allow_publishing(!_safe_dds_mode || (_safe_dds_mode && _offboard_mode_enabled));
-
 			if (ioctl(_fd, FIONREAD, (unsigned long)&bytes_available) == OK) {
 				if (bytes_available > 10) {
 					orb_poll_timeout_ms = 0;
@@ -41,7 +41,6 @@
 #include <uORB/topics/message_format_request.h>
 #include <uORB/topics/message_format_response.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_status.h>

 #include <lib/timesync/Timesync.hpp>

@@ -139,7 +138,6 @@ private:

 	uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
 	uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

 	/** Synchronizes the system clock if the time is off by more than 5 seconds */
 	void syncSystemClock(uxrSession *session);
@@ -204,8 +202,6 @@ private:
 	bool _connected{false};
 	bool _session_created{false};
 	bool _timesync_converged{false};
-	bool _offboard_mode_enabled{false};
-	bool _safe_dds_mode{true};

 	Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS};

@@ -220,7 +216,6 @@ private:
 		(ParamInt<px4::params::UXRCE_DDS_SYNCT>) _param_uxrce_dds_synct,
 		(ParamInt<px4::params::UXRCE_DDS_TX_TO>) _param_uxrce_dds_tx_to,
 		(ParamInt<px4::params::UXRCE_DDS_RX_TO>) _param_uxrce_dds_rx_to,
-		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl,
-		(ParamInt<px4::params::UXRCE_DDS_SAFE>) _param_uxrce_dds_safe
+		(ParamInt<px4::params::UXRCE_DDS_FLCTRL>) _param_uxrce_dds_flctrl
 	)
 };