diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 0084a6701e..7cb43adc91 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/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 diff --git a/boards/px4/sitl/simplesim.px4board b/boards/px4/sitl/simplesim.px4board new file mode 100644 index 0000000000..bb609ac0ee --- /dev/null +++ b/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 diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index b349475593..03737f89d0 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/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, diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 20eed499b5..0116c39142 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/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::infinity(); + local_position.vz_max = std::numeric_limits::infinity(); + local_position.hagl_min = std::numeric_limits::infinity(); + local_position.hagl_max = std::numeric_limits::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) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index 7790622c5e..02d965df7d 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -253,6 +253,15 @@ private: uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; + + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + uORB::Publication _vehicle_attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + + + //rpm uORB::Publication _rpm_pub{ORB_ID(rpm)};