[WIP] simple simulation without sensor/estimator

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -253,7 +253,7 @@ fi
 tone_alarm start
 rc_update start
 manual_control start
-sensors start
+#sensors start
 commander start
 
 if ! pwm_out_sim start -m sim

boards/px4/sitl/simplesim.px4board

@@ -0,0 +1,5 @@
+CONFIG_EKF2_AUX_GLOBAL_POSITION=n
+CONFIG_EKF2_VERBOSE_STATUS=n
+CONFIG_MODULES_EKF2=n
+CONFIG_MODULES_SENSORS=y
+CONFIG_BOARD_NOLOCKSTEP=y

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -156,18 +156,18 @@ private:
 #ifndef CONSTRAINED_FLASH
 		&_external_checks,
 #endif
-		&_accelerometer_checks,
-		&_airspeed_checks,
+		//&_accelerometer_checks,
+		//&_airspeed_checks,
 		&_arm_permission_checks,
-		&_baro_checks,
+		//&_baro_checks,
 		&_cpu_resource_checks,
 		&_distance_sensor_checks,
 		&_esc_checks,
 		&_estimator_checks,
 		&_failure_detector_checks,
-		&_gyro_checks,
-		&_imu_consistency_checks,
-		&_magnetometer_checks,
+		//&_gyro_checks,
+		//&_imu_consistency_checks,
+		//&_magnetometer_checks,
 		&_manual_control_checks,
 		&_home_position_checks,
 		&_mission_checks,

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -531,6 +531,124 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
+
+	uint64_t timestamp_sample = hrt_absolute_time(); // TODO: HIL_STATE_QUATERNION.time_us
+
+	matrix::Quatf q(hil_state.attitude_quaternion);
+	q.normalize();
+
+	const matrix::Eulerf euler(q);
+
+	// angular velocity
+	{
+		vehicle_angular_velocity_s angular_velocity{};
+		angular_velocity.timestamp_sample = timestamp_sample;
+		angular_velocity.xyz[0] = hil_state.rollspeed;
+		angular_velocity.xyz[1] = hil_state.pitchspeed;
+		angular_velocity.xyz[2] = hil_state.yawspeed;
+
+		// TODO: add derivative fields
+		// angular_velocity.xyz_derivative[0] = hil_state.rollspeed;
+		// angular_velocity.xyz_derivative[1] = hil_state.pitchspeed;
+		// angular_velocity.xyz_derivative[2] = hil_state.yawspeed;
+
+		angular_velocity.timestamp = hrt_absolute_time();
+		_vehicle_angular_velocity_pub.publish(angular_velocity);
+	}
+
+	// vehicle_acceleration // TODO:
+
+	// vehicle_attitude
+	{
+		vehicle_attitude_s attitude{};
+		attitude.timestamp_sample = timestamp_sample;
+
+		q.copyTo(attitude.q);
+
+		attitude.timestamp = hrt_absolute_time();
+		_vehicle_attitude_pub.publish(attitude);
+	}
+
+	// vehicle_local_position
+	{
+		vehicle_local_position_s local_position{};
+		local_position.timestamp_sample = timestamp_sample;
+
+
+		double lat = hil_state.lat * 1e-7; // degE7 -> deg
+		double lon = hil_state.lon * 1e-7; // degE7 -> deg
+
+		if (!_global_local_proj_ref.isInitialized()) {
+			_global_local_proj_ref.initReference(lat, lon);
+			_global_local_alt0 = hil_state.alt / 1000.f; // mm -> m
+		}
+
+		local_position.xy_valid = true;
+		local_position.z_valid = true;
+		local_position.v_xy_valid = true;
+		local_position.v_z_valid = true;
+
+		_global_local_proj_ref.project(lat, lon, local_position.x, local_position.y);
+		local_position.z = _global_local_alt0 - (hil_state.alt / 1000.f); // mm -> m
+
+		local_position.vx = hil_state.vx / 100.f; // cm/s -> m/s
+		local_position.vy = hil_state.vy / 100.f; // cm/s -> m/s
+		local_position.vz = hil_state.vz / 100.f; // cm/s -> m/s
+		local_position.z_deriv = local_position.vz;
+
+		local_position.ax = hil_state.xacc / 1000.f * CONSTANTS_ONE_G; // mG -> m/s/s
+		local_position.ay = hil_state.yacc / 1000.f * CONSTANTS_ONE_G; // mG -> m/s/s
+		local_position.az = hil_state.zacc / 1000.f * CONSTANTS_ONE_G; // mG -> m/s/s
+
+		local_position.heading = euler.psi();
+		local_position.unaided_heading = local_position.heading;
+
+		local_position.xy_global = true;
+		local_position.z_global = true;
+		local_position.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp();
+		local_position.ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
+		local_position.ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
+		local_position.ref_alt = _global_local_alt0;
+
+		local_position.dist_bottom = NAN;
+
+		local_position.eph = 1.f;
+		local_position.epv = 2.f;
+		local_position.evh = 1.f;
+		local_position.evv = 1.f;
+
+		local_position.vxy_max = std::numeric_limits<float>::infinity();
+		local_position.vz_max = std::numeric_limits<float>::infinity();
+		local_position.hagl_min = std::numeric_limits<float>::infinity();
+		local_position.hagl_max = std::numeric_limits<float>::infinity();
+
+		local_position.timestamp = hrt_absolute_time();
+		_vehicle_local_position_pub.publish(local_position);
+	}
+
+
+	// global position
+	{
+		vehicle_global_position_s global_position{};
+		global_position.timestamp_sample = timestamp_sample;
+
+		global_position.lat = hil_state.lat / 1e7;
+		global_position.lon = hil_state.lon / 1e7;
+		global_position.alt = hil_state.alt / 1e3;
+		global_position.alt_ellipsoid = global_position.alt;
+
+		global_position.eph = 1.f;
+		global_position.epv = 2.f;
+
+		global_position.terrain_alt = NAN;
+
+		global_position.timestamp = hrt_absolute_time();
+		_vehicle_global_position_pub.publish(global_position);
+	}
+
+
+#if 0
+
 	uint64_t timestamp = hrt_absolute_time();
 
 	/* angular velocity */
@@ -614,6 +732,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 		// always publish ground truth attitude message
 		_lpos_ground_truth_pub.publish(hil_lpos);
 	}
+#endif
+
 }
 
 void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *msg)

src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp

@@ -253,6 +253,15 @@ private:
 	uORB::Publication<vehicle_local_position_s>	_lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
 	uORB::Publication<input_rc_s>			_input_rc_pub{ORB_ID(input_rc)};
 
+
+	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Publication<vehicle_attitude_s>         _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
+	uORB::Publication<vehicle_global_position_s>  _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
+	uORB::Publication<vehicle_local_position_s>   _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
+	uORB::Publication<vehicle_odometry_s>         _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
+
+
+
 	//rpm
 	uORB::Publication<rpm_s>			_rpm_pub{ORB_ID(rpm)};