Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

simulator sih add local position ground truth and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2022-08-31 10:56:25 -04:00
parent 355f184f06
commit 99a20646e2
2 changed files with 253 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/simulator_sih/sih.cpp
+198 -178
View File
File diff suppressed because it is too large Load Diff
src/modules/simulation/simulator_sih/sih.hpp
+55 -72
View File
@@ -66,19 +66,20 @@
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <perf/perf_counter.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_angular_velocity.h> // to publish groundtruth
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
#include <sys/time.h>
@@ -86,13 +87,10 @@
using namespace time_literals;
extern "C" __EXPORT int sih_main(int argc, char *argv[]);
class Sih : public ModuleBase<Sih>, public ModuleParams
{
public:
Sih();
virtual ~Sih();
/** @see ModuleBase */
@@ -118,41 +116,23 @@ public:
// generate white Gaussian noise sample as a 3D vector with specified std
static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
// timer called periodically to post the semaphore
static void timer_callback(void *sem);
private:
void parameters_updated();
// simulated sensor instances
// simulated sensors
PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<distance_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
// to publish the gps position
sensor_gps_s _sensor_gps{};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
// to publish the distance sensor
distance_sensor_s _distance_snsr{};
uORB::Publication<distance_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
// angular velocity groundtruth
vehicle_angular_velocity_s _vehicle_angular_velocity_gt{};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_gt_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
// attitude groundtruth
vehicle_attitude_s _att_gt{};
uORB::Publication<vehicle_attitude_s> _att_gt_pub{ORB_ID(vehicle_attitude_groundtruth)};
// global position groundtruth
vehicle_global_position_s _gpos_gt{};
uORB::Publication<vehicle_global_position_s> _gpos_gt_pub{ORB_ID(vehicle_global_position_groundtruth)};
// airspeed
uORB::Publication<airspeed_s> _airspeed_pub{ORB_ID(airspeed)};
// groundtruth
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
uORB::Publication<vehicle_local_position_s> _local_position_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
uORB::Publication<vehicle_global_position_s> _global_position_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
@@ -161,7 +141,7 @@ private:
static constexpr uint16_t NB_MOTORS = 6;
static constexpr float T1_C = 15.0f; // ground temperature in Celsius
static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // ground temperature in Kelvin
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
// Aerodynamic coefficients
static constexpr float RHO = 1.225f; // air density at sea level [kg/m^3]
static constexpr float SPAN = 0.86f; // wing span [m]
@@ -170,16 +150,22 @@ private:
static constexpr float FLAP_MAX = M_PI_F / 12.0f; // 15 deg, maximum control surface deflection
void init_variables();
void gps_fix();
void gps_no_fix();
void read_motors();
// read the motor signals outputted from the mixer
void read_motors(const float dt);
// generate the motors thrust and torque in the body frame
void generate_force_and_torques();
void equations_of_motion();
void reconstruct_sensors_signals();
void send_gps();
void send_airspeed();
void send_dist_snsr();
void publish_sih();
// apply the equations of motion of a rigid body and integrate one step
void equations_of_motion(const float dt);
// reconstruct the noisy sensor signals
void reconstruct_sensors_signals(const hrt_abstime &time_now_us);
void send_gps(const hrt_abstime &time_now_us);
void send_airspeed(const hrt_abstime &time_now_us);
void send_dist_snsr(const hrt_abstime &time_now_us);
void publish_ground_truth(const hrt_abstime &time_now_us);
void generate_fw_aerodynamics();
void generate_ts_aerodynamics();
void sensor_step();
@@ -191,11 +177,12 @@ private:
#endif
void realtime_loop();
px4_sem_t _data_semaphore;
hrt_call _timer_call;
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
px4_sem_t _data_semaphore;
hrt_call _timer_call{};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
hrt_abstime _last_run{0};
hrt_abstime _last_actuator_output_time{0};
@@ -203,27 +190,25 @@ private:
hrt_abstime _gps_time{0};
hrt_abstime _airspeed_time{0};
hrt_abstime _mag_time{0};
hrt_abstime _gt_time{0};
hrt_abstime _dist_snsr_time{0};
hrt_abstime _now{0};
float _dt{0}; // sampling time [s]
bool _grounded{true};// whether the vehicle is on the ground
matrix::Vector3f _T_B; // thrust force in body frame [N]
matrix::Vector3f _Fa_I; // aerodynamic force in inertial frame [N]
matrix::Vector3f _Mt_B; // thruster moments in the body frame [Nm]
matrix::Vector3f _Ma_B; // aerodynamic moments in the body frame [Nm]
matrix::Vector3f _p_I; // inertial position [m]
matrix::Vector3f _v_I; // inertial velocity [m/s]
matrix::Vector3f _v_B; // body frame velocity [m/s]
matrix::Vector3f _p_I_dot; // inertial position differential
matrix::Vector3f _v_I_dot; // inertial velocity differential
matrix::Quatf _q; // quaternion attitude
matrix::Dcmf _C_IB; // body to inertial transformation
matrix::Vector3f _w_B; // body rates in body frame [rad/s]
matrix::Quatf _dq; // quaternion differential
matrix::Vector3f _w_B_dot; // body rates differential
float _u[NB_MOTORS]; // thruster signals
matrix::Vector3f _T_B{}; // thrust force in body frame [N]
matrix::Vector3f _Fa_I{}; // aerodynamic force in inertial frame [N]
matrix::Vector3f _Mt_B{}; // thruster moments in the body frame [Nm]
matrix::Vector3f _Ma_B{}; // aerodynamic moments in the body frame [Nm]
matrix::Vector3f _p_I{}; // inertial position [m]
matrix::Vector3f _v_I{}; // inertial velocity [m/s]
matrix::Vector3f _v_B{}; // body frame velocity [m/s]
matrix::Vector3f _p_I_dot{}; // inertial position differential
matrix::Vector3f _v_I_dot{}; // inertial velocity differential
matrix::Quatf _q{}; // quaternion attitude
matrix::Dcmf _C_IB{}; // body to inertial transformation
matrix::Vector3f _w_B{}; // body rates in body frame [rad/s]
matrix::Quatf _dq{}; // quaternion differential
matrix::Vector3f _w_B_dot{}; // body rates differential
float _u[NB_MOTORS] {}; // thruster signals
enum class VehicleType {MC, FW, TS};
VehicleType _vehicle = VehicleType::MC;
@@ -273,10 +258,8 @@ private:
// };
// sensors reconstruction
matrix::Vector3f _acc;
matrix::Vector3f _mag;
matrix::Vector3f _gyro;
matrix::Vector3f _gps_vel;
matrix::Vector3f _mag{};
matrix::Vector3f _gps_vel{};
double _gps_lat, _gps_lat_noiseless;
double _gps_lon, _gps_lon_noiseless;
float _gps_alt, _gps_alt_noiseless;