45 #include <px4_platform_common/getopt.h> 46 #include <px4_platform_common/log.h> 74 _task_id = px4_task_spawn_cmd(
"sih",
78 (px4_main_t)&run_trampoline,
93 if (instance ==
nullptr) {
94 PX4_ERR(
"alloc failed");
101 ModuleParams(nullptr),
129 while (!should_exit()) {
150 px4_sem_post((px4_sem_t *)sem);
202 _T_MAX = _sih_t_max.get();
203 _Q_MAX = _sih_q_max.get();
206 _KDV = _sih_kdv.get();
207 _KDW = _sih_kdw.get();
210 _LAT0 = (double)_sih_lat0.get() * 1.0e-7;
211 _LON0 = (double)_sih_lon0.get() * 1.0e-7;
214 _MASS = _sih_mass.get();
218 _I =
diag(
Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get()));
219 _I(0, 1) =
_I(1, 0) = _sih_ixy.get();
220 _I(0, 2) =
_I(2, 0) = _sih_ixz.get();
221 _I(1, 2) =
_I(2, 1) = _sih_iyz.get();
225 _mu_I =
Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
238 _u[0] =
_u[1] =
_u[2] =
_u[3] = 0.0f;
261 bool updated =
false;
354 static constexpr
float scaling = 1000.0f;
362 static constexpr
float scaling = 1000.0f;
370 static constexpr
float scaling = 1000.0f;
460 static float V1, V2, S;
461 static bool phase =
true;
466 float U1 = (float)rand() / RAND_MAX;
467 float U2 = (float)rand() / RAND_MAX;
468 V1 = 2.0f * U1 - 1.0f;
469 V2 = 2.0f * U2 - 1.0f;
470 S = V1 * V1 + V2 * V2;
471 }
while (S >= 1.0
f || fabsf(S) < 1e-8
f);
473 X = V1 * float(sqrtf(-2.0
f *
float(logf(S)) / S));
476 X = V2 * float(sqrtf(-2.0
f *
float(logf(S)) / S));
497 PX4_WARN(
"%s\n", reason);
500 PRINT_MODULE_DESCRIPTION(
503 This module provide a simulator for quadrotors running fully 504 inside the hardware autopilot. 506 This simulator subscribes to "actuator_outputs" which are the actuator pwm 507 signals given by the mixer. 509 This simulator publishes the sensors signals corrupted with realistic noise 510 in order to incorporate the state estimator in the loop. 513 The simulator implements the equations of motion using matrix algebra. 514 Quaternion representation is used for the attitude. 515 Forward Euler is used for integration. 516 Most of the variables are declared global in the .hpp file to avoid stack overflow. 521 PRINT_MODULE_USAGE_NAME("sih",
"simulation");
522 PRINT_MODULE_USAGE_COMMAND(
"start");
523 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
#define PWM_DEFAULT_MAX
Default maximum PWM in us.
uORB::Subscription _parameter_update_sub
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
perf_counter_t _sampling_perf
static int task_spawn(int argc, char *argv[])
Vector3 cross(const Matrix31 &b) const
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
static constexpr float CONSTANTS_AIR_GAS_CONST
void set_temperature(float temperature)
static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS
measure the time elapsed performing an event
static constexpr double CONSTANTS_RADIUS_OF_EARTH
void equations_of_motion()
static float generate_wgn()
static constexpr uint16_t NB_MOTORS
Dcm< Type > to_dcm()
XXX DEPRECATED, can use assignment or ctor.
void update(hrt_abstime timestamp, float x, float y, float z)
matrix::Vector3f _w_B_dot
int main(int argc, char **argv)
vehicle_global_position_s _gpos_gt
static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz)
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
static constexpr float T1_C
static void print_usage()
int print_status() override
constexpr T degrees(T radians)
Matrix< Type, N, M > transpose() const
orb_advert_t _vehicle_angular_velocity_gt_pub
static constexpr float CONSTANTS_ONE_G
static int print_usage(const char *reason=nullptr)
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void update(hrt_abstime timestamp, float x, float y, float z)
PX4Accelerometer _px4_accel
vehicle_attitude_s _att_gt
static constexpr hrt_abstime LOOP_INTERVAL
void set_temperature(float temperature)
double _gps_lat_noiseless
void set_temperature(float temperature)
Matrix41 derivative1(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 1 v_2 = q_21 * ...
void reconstruct_sensors_signals()
void update(hrt_abstime timestamp, float pressure)
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
static constexpr float TEMP_GRADIENT
int orb_unsubscribe(int handle)
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
matrix::Vector3f _v_I_dot
static constexpr float T1_K
void perf_end(perf_counter_t handle)
End a performance event.
orb_advert_t _gpos_gt_pub
static Sih * instantiate(int argc, char *argv[])
__EXPORT void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
Call callout(arg) after delay, and then after every interval.
bool updated()
Check if there is a new update.
double _gps_lon_noiseless
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
int sih_main(int argc, char *argv[])
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
px4_sem_t _data_semaphore
matrix::Vector3f _p_I_dot
SquareMatrix< Type, M > diag(Vector< Type, M > d)
void set_scale(float scale)
void set_scale(float scale)
perf_counter_t _loop_perf
Vector3< float > Vector3f
void set_temperature(float temperature)
void parameters_updated()
void parameters_update_poll()
Check for parameter changes and update them if needed.
Quaternion< float > Quatf
int orb_check(int handle, bool *updated)
matrix::Vector3f _gps_vel
void generate_force_and_torques()
static int custom_command(int argc, char *argv[])
static constexpr float CONSTANTS_STD_PRESSURE_MBAR
void set_scale(float scale)
orb_advert_t _vehicle_gps_pos_pub
#define PWM_DEFAULT_MIN
Default minimum PWM in us.
bool copy(void *dst)
Copy the struct.
vehicle_gps_position_s _vehicle_gps_pos
void perf_begin(perf_counter_t handle)
Begin a performance event.
vehicle_angular_velocity_s _vehicle_angular_velocity_gt
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
__EXPORT void hrt_cancel(struct hrt_call *entry)
Remove the entry from the callout list.
static void timer_callback(void *sem)