PX4 Firmware
PX4 Autopilot Software http://px4.io
sih.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file sih.cpp
36  * Simulator in Hardware
37  *
38  * @author Romain Chiappinelli <romain.chiap@gmail.com>
39  *
40  * Coriolis g Corporation - January 2019
41  */
42 
43 #include "sih.hpp"
44 
45 #include <px4_platform_common/getopt.h>
46 #include <px4_platform_common/log.h>
47 
48 #include <drivers/drv_pwm_output.h> // to get PWM flags
50 #include <uORB/topics/vehicle_status.h> // to get the HIL status
51 
52 #include <unistd.h>
53 #include <string.h>
54 #include <fcntl.h>
55 #include <termios.h>
56 
57 using namespace math;
58 using namespace matrix;
59 
61 {
62  PX4_INFO("Running");
63  return 0;
64 }
65 
66 int Sih::custom_command(int argc, char *argv[])
67 {
68  return print_usage("unknown command");
69 }
70 
71 
72 int Sih::task_spawn(int argc, char *argv[])
73 {
74  _task_id = px4_task_spawn_cmd("sih",
75  SCHED_DEFAULT,
76  SCHED_PRIORITY_MAX,
77  1024,
78  (px4_main_t)&run_trampoline,
79  (char *const *)argv);
80 
81  if (_task_id < 0) {
82  _task_id = -1;
83  return -errno;
84  }
85 
86  return 0;
87 }
88 
89 Sih *Sih::instantiate(int argc, char *argv[])
90 {
91  Sih *instance = new Sih();
92 
93  if (instance == nullptr) {
94  PX4_ERR("alloc failed");
95  }
96 
97  return instance;
98 }
99 
101  ModuleParams(nullptr),
102  _loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
103  _sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
104 {
105 }
106 
107 void Sih::run()
108 {
109  // to subscribe to (read) the actuators_out pwm
110  _actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs));
111 
112  // initialize parameters
114 
115  init_variables();
116  init_sensors();
117 
118  const hrt_abstime task_start = hrt_absolute_time();
119  _last_run = task_start;
120  _gps_time = task_start;
121  _serial_time = task_start;
122 
123  px4_sem_init(&_data_semaphore, 0, 0);
124 
126 
128 
129  while (!should_exit()) {
130  px4_sem_wait(&_data_semaphore); // periodic real time wakeup
131 
134 
136 
137  inner_loop(); // main execution function
138 
140  }
141 
142  hrt_cancel(&_timer_call); // close the periodic timer interruption
143  px4_sem_destroy(&_data_semaphore);
145 }
146 
147 // timer_callback() is used as a real time callback to post the semaphore
148 void Sih::timer_callback(void *sem)
149 {
150  px4_sem_post((px4_sem_t *)sem);
151 }
152 
153 // this is the main execution waken up periodically by the semaphore
155 {
157  _dt = (_now - _last_run) * 1e-6f;
158  _last_run = _now;
159 
160  read_motors();
161 
163 
165 
167 
168  send_IMU();
169 
170  if (_now - _gps_time >= 50000) { // gps published at 20Hz
171  _gps_time = _now;
172  send_gps();
173  }
174 
175  // send uart message every 40 ms
176  if (_now - _serial_time >= 40000) {
177  _serial_time = _now;
178 
179  publish_sih(); // publish _sih message for debug purpose
180 
181  parameters_update_poll(); // update the parameters if needed
182  }
183 }
184 
186 {
187  // check for parameter updates
189  // clear update
190  parameter_update_s pupdate;
191  _parameter_update_sub.copy(&pupdate);
192 
193  // update parameters from storage
194  updateParams();
196  }
197 }
198 
199 // store the parameters in a more convenient form
201 {
202  _T_MAX = _sih_t_max.get();
203  _Q_MAX = _sih_q_max.get();
204  _L_ROLL = _sih_l_roll.get();
205  _L_PITCH = _sih_l_pitch.get();
206  _KDV = _sih_kdv.get();
207  _KDW = _sih_kdw.get();
208  _H0 = _sih_h0.get();
209 
210  _LAT0 = (double)_sih_lat0.get() * 1.0e-7;
211  _LON0 = (double)_sih_lon0.get() * 1.0e-7;
212  _COS_LAT0 = cosl(radians(_LAT0));
213 
214  _MASS = _sih_mass.get();
215 
216  _W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G);
217 
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();
222 
223  _Im1 = inv(_I);
224 
225  _mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
226 }
227 
228 // initialization of the variables for the simulator
230 {
231  srand(1234); // initialize the random seed once before calling generate_wgn()
232 
233  _p_I = Vector3f(0.0f, 0.0f, 0.0f);
234  _v_I = Vector3f(0.0f, 0.0f, 0.0f);
235  _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
236  _w_B = Vector3f(0.0f, 0.0f, 0.0f);
237 
238  _u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
239 }
240 
242 {
243  _vehicle_gps_pos.fix_type = 3; // 3D fix
249  _vehicle_gps_pos.eph = 0.9f;
250  _vehicle_gps_pos.epv = 1.78f;
251  _vehicle_gps_pos.hdop = 0.7f;
252  _vehicle_gps_pos.vdop = 1.1f;
253 }
254 
255 // read the motor signals outputted from the mixer
257 {
258  actuator_outputs_s actuators_out {};
259 
260  // read the actuator outputs
261  bool updated = false;
262  orb_check(_actuator_out_sub, &updated);
263 
264  if (updated) {
265  orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out);
266 
267  for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
268  _u[i] = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
269  }
270  }
271 }
272 
273 // generate the motors thrust and torque in the body frame
275 {
276  _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
277  _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
278  _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
279  _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
280 
281  _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
282  _Ma_B = -_KDW * _w_B; // first order angular damper
283 }
284 
285 // apply the equations of motion of a rigid body and integrate one step
287 {
288  _C_IB = _q.to_dcm(); // body to inertial transformation
289 
290  // Equations of motion of a rigid body
291  _p_I_dot = _v_I; // position differential
292  _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum
293  _q_dot = _q.derivative1(_w_B); // attitude differential
294  _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum
295 
296  // fake ground, avoid free fall
297  if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
298  if (!_grounded) { // if we just hit the floor
299  // for the accelerometer, compute the acceleration that will stop the vehicle in one time step
300  _v_I_dot = -_v_I / _dt;
301 
302  } else {
303  _v_I_dot.setZero();
304  }
305 
306  _v_I.setZero();
307  _w_B.setZero();
308  _grounded = true;
309 
310  } else {
311  // integration: Euler forward
312  _p_I = _p_I + _p_I_dot * _dt;
313  _v_I = _v_I + _v_I_dot * _dt;
314  _q = _q + _q_dot * _dt; // as given in attitude_estimator_q_main.cpp
315  _q.normalize();
316  _w_B = _w_B + _w_B_dot * _dt;
317  _grounded = false;
318  }
319 }
320 
321 // reconstruct the noisy sensor signals
323 {
324  // The sensor signals reconstruction and noise levels are from
325  // Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
326  // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
327 
328  // IMU
329  _acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
330  _gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
331  _mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
332 
333  // barometer
334  float altitude = (_H0 - _p_I(2)) + generate_wgn() * 0.14f; // altitude with noise
335  _baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar
336  powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST));
337  _baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in celcius
338 
339  // GPS
342  _gps_alt_noiseless = _H0 - _p_I(2);
343 
344  _gps_lat = _gps_lat_noiseless + (double)(generate_wgn() * 7.2e-6f); // latitude in degrees
345  _gps_lon = _gps_lon_noiseless + (double)(generate_wgn() * 1.75e-5f); // longitude in degrees
347  _gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f);
348 }
349 
351 {
352  // gyro
353  {
354  static constexpr float scaling = 1000.0f;
355  _px4_gyro.set_scale(1 / scaling);
357  _px4_gyro.update(_now, _gyro(0) * scaling, _gyro(1) * scaling, _gyro(2) * scaling);
358  }
359 
360  // accel
361  {
362  static constexpr float scaling = 1000.0f;
363  _px4_accel.set_scale(1 / scaling);
365  _px4_accel.update(_now, _acc(0) * scaling, _acc(1) * scaling, _acc(2) * scaling);
366  }
367 
368  // magnetometer
369  {
370  static constexpr float scaling = 1000.0f;
371  _px4_mag.set_scale(1 / scaling);
373  _px4_mag.update(_now, _mag(0) * scaling, _mag(1) * scaling, _mag(2) * scaling);
374  }
375 
376  // baro
377  {
380  }
381 }
382 
384 {
386  _vehicle_gps_pos.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees
387  _vehicle_gps_pos.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
388  _vehicle_gps_pos.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
389  _vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
390  _vehicle_gps_pos.vel_ned_valid = true; // True if NED velocity is valid
392  1)); // GPS ground speed, (metres/sec)
393  _vehicle_gps_pos.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec)
394  _vehicle_gps_pos.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec)
395  _vehicle_gps_pos.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec)
397  _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
398 
399  if (_vehicle_gps_pos_pub != nullptr) {
400  orb_publish(ORB_ID(vehicle_gps_position), _vehicle_gps_pos_pub, &_vehicle_gps_pos);
401 
402  } else {
403  _vehicle_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_vehicle_gps_pos);
404  }
405 }
406 
408 {
409  // publish angular velocity groundtruth
411  _vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed;
412  _vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed;
413  _vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed;
414 
415  if (_vehicle_angular_velocity_gt_pub != nullptr) {
416  orb_publish(ORB_ID(vehicle_angular_velocity_groundtruth), _vehicle_angular_velocity_gt_pub,
418 
419  } else {
420  _vehicle_angular_velocity_gt_pub = orb_advertise(ORB_ID(vehicle_angular_velocity_groundtruth),
422  }
423 
424  // publish attitude groundtruth
426  _att_gt.q[0] = _q(0);
427  _att_gt.q[1] = _q(1);
428  _att_gt.q[2] = _q(2);
429  _att_gt.q[3] = _q(3);
430 
431  if (_att_gt_pub != nullptr) {
432  orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_pub, &_att_gt);
433 
434  } else {
435  _att_gt_pub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
436  }
437 
442  _gpos_gt.vel_n = _v_I(0);
443  _gpos_gt.vel_e = _v_I(1);
444  _gpos_gt.vel_d = _v_I(2);
445 
446  if (_gpos_gt_pub != nullptr) {
447  orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_pub, &_gpos_gt);
448 
449  } else {
450  _gpos_gt_pub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
451  }
452 }
453 
454 float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
455 {
456  // algorithm 1:
457  // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
458  // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
459  // algorithm 2: from BlockRandGauss.hpp
460  static float V1, V2, S;
461  static bool phase = true;
462  float X;
463 
464  if (phase) {
465  do {
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.0f || fabsf(S) < 1e-8f);
472 
473  X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
474 
475  } else {
476  X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
477  }
478 
479  phase = !phase;
480  return X;
481 }
482 
483 // generate white Gaussian noise sample vector with specified std
484 Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
485 {
486  return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
487 }
488 
489 int sih_main(int argc, char *argv[])
490 {
491  return Sih::main(argc, argv);
492 }
493 
494 int Sih::print_usage(const char *reason)
495 {
496  if (reason) {
497  PX4_WARN("%s\n", reason);
498  }
499 
500  PRINT_MODULE_DESCRIPTION(
501  R"DESCR_STR(
502 ### Description
503 This module provide a simulator for quadrotors running fully
504 inside the hardware autopilot.
505 
506 This simulator subscribes to "actuator_outputs" which are the actuator pwm
507 signals given by the mixer.
508 
509 This simulator publishes the sensors signals corrupted with realistic noise
510 in order to incorporate the state estimator in the loop.
511 
512 ### Implementation
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.
517 
518 
519 )DESCR_STR");
520 
521  PRINT_MODULE_USAGE_NAME("sih", "simulation");
522  PRINT_MODULE_USAGE_COMMAND("start");
523  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
524 
525  return 0;
526 }
#define PWM_DEFAULT_MAX
Default maximum PWM in us.
uORB::Subscription _parameter_update_sub
Definition: sih.hpp:123
float _gps_alt_noiseless
Definition: sih.hpp:181
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
matrix::Matrix3f _I
Definition: sih.hpp:189
perf_counter_t _sampling_perf
Definition: sih.hpp:145
static int task_spawn(int argc, char *argv[])
Definition: sih.cpp:72
Vector3 cross(const Matrix31 &b) const
Definition: Vector3.hpp:59
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
static constexpr float CONSTANTS_AIR_GAS_CONST
Definition: geo.h:58
void set_temperature(float temperature)
static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS
Definition: geo.h:59
void send_gps()
Definition: sih.cpp:383
float _L_ROLL
Definition: sih.hpp:186
measure the time elapsed performing an event
Definition: perf_counter.h:56
hrt_abstime _now
Definition: sih.hpp:153
matrix::Vector3f _Mt_B
Definition: sih.hpp:159
float _u[NB_MOTORS]
Definition: sih.hpp:171
static constexpr double CONSTANTS_RADIUS_OF_EARTH
Definition: geo.h:61
void equations_of_motion()
Definition: sih.cpp:286
static float generate_wgn()
Definition: sih.cpp:454
static constexpr uint16_t NB_MOTORS
Definition: sih.hpp:127
matrix::Vector3f _acc
Definition: sih.hpp:175
Dcm< Type > to_dcm()
XXX DEPRECATED, can use assignment or ctor.
Definition: Quaternion.hpp:538
float _KDW
Definition: sih.hpp:186
void update(hrt_abstime timestamp, float x, float y, float z)
matrix::Vector3f _w_B_dot
Definition: sih.hpp:170
int main(int argc, char **argv)
Definition: main.cpp:3
matrix::Quatf _q
Definition: sih.hpp:166
void inner_loop()
Definition: sih.cpp:154
vehicle_global_position_s _gpos_gt
Definition: sih.hpp:120
static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz)
Definition: sih.cpp:484
orb_advert_t _att_gt_pub
Definition: sih.hpp:117
float _T_MAX
Definition: sih.hpp:186
matrix::Vector3f _T_B
Definition: sih.hpp:157
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
static constexpr float T1_C
Definition: sih.hpp:128
void normalize()
Definition: Vector.hpp:87
Sih()
Definition: sih.cpp:100
static void print_usage()
matrix::Vector3f _W_I
Definition: sih.hpp:188
matrix::Matrix3f _Im1
Definition: sih.hpp:190
LidarLite * instance
Definition: ll40ls.cpp:65
int print_status() override
Definition: sih.cpp:60
constexpr T degrees(T radians)
Definition: Limits.hpp:91
Matrix< Type, N, M > transpose() const
Definition: Matrix.hpp:353
orb_advert_t _vehicle_angular_velocity_gt_pub
Definition: sih.hpp:113
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
float _MASS
Definition: sih.hpp:186
matrix::Vector3f _w_B
Definition: sih.hpp:168
matrix::Vector3f _mu_I
Definition: sih.hpp:191
float _H0
Definition: sih.hpp:186
static int print_usage(const char *reason=nullptr)
Definition: sih.cpp:494
float _dt
Definition: sih.hpp:154
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void update(hrt_abstime timestamp, float x, float y, float z)
double _COS_LAT0
Definition: sih.hpp:187
PX4Accelerometer _px4_accel
Definition: sih.hpp:102
void run() override
Definition: sih.cpp:107
PX4Gyroscope _px4_gyro
Definition: sih.hpp:103
vehicle_attitude_s _att_gt
Definition: sih.hpp:116
static constexpr hrt_abstime LOOP_INTERVAL
Definition: sih.hpp:131
void publish_sih()
Definition: sih.cpp:407
void set_temperature(float temperature)
double _gps_lat_noiseless
Definition: sih.hpp:179
void set_temperature(float temperature)
bool _grounded
Definition: sih.hpp:155
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 * ...
Definition: Quaternion.hpp:308
hrt_abstime _last_run
Definition: sih.hpp:150
#define perf_alloc(a, b)
Definition: px4io.h:59
void reconstruct_sensors_signals()
Definition: sih.cpp:322
void update(hrt_abstime timestamp, float pressure)
PX4Magnetometer _px4_mag
Definition: sih.hpp:104
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
static constexpr float TEMP_GRADIENT
Definition: sih.hpp:130
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
int _actuator_out_sub
Definition: sih.hpp:124
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
matrix::Vector3f _p_I
Definition: sih.hpp:161
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
void send_IMU()
Definition: sih.cpp:350
matrix::Vector3f _v_I_dot
Definition: sih.hpp:165
static constexpr float T1_K
Definition: sih.hpp:129
float _gps_alt
Definition: sih.hpp:181
float _baro_p_mBar
Definition: sih.hpp:182
void perf_end(perf_counter_t handle)
End a performance event.
orb_advert_t _gpos_gt_pub
Definition: sih.hpp:121
void read_motors()
Definition: sih.cpp:256
static Sih * instantiate(int argc, char *argv[])
Definition: sih.cpp:89
__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
Definition: sih.hpp:180
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
double _LAT0
Definition: sih.hpp:187
int sih_main(int argc, char *argv[])
Definition: sih.cpp:489
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
px4_sem_t _data_semaphore
Definition: sih.hpp:147
matrix::Vector3f _p_I_dot
Definition: sih.hpp:164
SquareMatrix< Type, M > diag(Vector< Type, M > d)
void set_scale(float scale)
void set_scale(float scale)
float _L_PITCH
Definition: sih.hpp:186
matrix::Vector3f _Fa_I
Definition: sih.hpp:158
matrix::Quatf _q_dot
Definition: sih.hpp:169
perf_counter_t _loop_perf
Definition: sih.hpp:144
Vector3< float > Vector3f
Definition: Vector3.hpp:136
void set_temperature(float temperature)
matrix::Vector3f _gyro
Definition: sih.hpp:177
void init_variables()
Definition: sih.cpp:229
void parameters_updated()
Definition: sih.cpp:200
hrt_abstime _gps_time
Definition: sih.hpp:151
void parameters_update_poll()
Check for parameter changes and update them if needed.
Definition: sih.cpp:185
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
hrt_call _timer_call
Definition: sih.hpp:149
float _Q_MAX
Definition: sih.hpp:186
matrix::Vector3f _gps_vel
Definition: sih.hpp:178
float _KDV
Definition: sih.hpp:186
float _baro_temp_c
Definition: sih.hpp:183
Definition: sih.hpp:58
void generate_force_and_torques()
Definition: sih.cpp:274
matrix::Vector3f _v_I
Definition: sih.hpp:162
static int custom_command(int argc, char *argv[])
Definition: sih.cpp:66
static constexpr float CONSTANTS_STD_PRESSURE_MBAR
Definition: geo.h:55
matrix::Vector3f _Ma_B
Definition: sih.hpp:160
hrt_abstime _serial_time
Definition: sih.hpp:152
matrix::Dcmf _C_IB
Definition: sih.hpp:167
double _gps_lat
Definition: sih.hpp:179
void set_scale(float scale)
void setZero()
Definition: Matrix.hpp:416
orb_advert_t _vehicle_gps_pos_pub
Definition: sih.hpp:109
#define PWM_DEFAULT_MIN
Default minimum PWM in us.
bool copy(void *dst)
Copy the struct.
vehicle_gps_position_s _vehicle_gps_pos
Definition: sih.hpp:108
matrix::Vector3f _mag
Definition: sih.hpp:176
void perf_begin(perf_counter_t handle)
Begin a performance event.
double _LON0
Definition: sih.hpp:187
vehicle_angular_velocity_s _vehicle_angular_velocity_gt
Definition: sih.hpp:112
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:325
__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.
double _gps_lon
Definition: sih.hpp:180
static void timer_callback(void *sem)
Definition: sih.cpp:148
void init_sensors()
Definition: sih.cpp:241
PX4Barometer _px4_baro
Definition: sih.hpp:105