fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 23:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sih: fix code style

Browse Source
This commit is contained in:
Daniel Agar
2019-07-28 11:18:02 -04:00
parent 09eaef82f6
commit be99d2f111
1 changed files with 282 additions and 280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sih/sih.cpp
+282 -280
View File
@@ -57,334 +57,296 @@
using namespace math; using namespace math;
using namespace matrix; using namespace matrix;
int Sih::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module provide a simulator for quadrotors running fully
inside the hardware autopilot.
This simulator subscribes to "actuator_outputs" which are the actuator pwm
signals given by the mixer.
This simulator publishes the sensors signals corrupted with realistic noise
in order to incorporate the state estimator in the loop.
### Implementation
The simulator implements the equations of motion using matrix algebra.
Quaternion representation is used for the attitude.
Forward Euler is used for integration.
Most of the variables are declared global in the .hpp file to avoid stack overflow.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("sih", "simulation");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int Sih::print_status() int Sih::print_status()
{ {
PX4_INFO("Running"); PX4_INFO("Running");
return 0; return 0;
} }
int Sih::custom_command(int argc, char *argv[]) int Sih::custom_command(int argc, char *argv[])
{ {
return print_usage("unknown command"); return print_usage("unknown command");
} }
int Sih::task_spawn(int argc, char *argv[]) int Sih::task_spawn(int argc, char *argv[])
{ {
_task_id = px4_task_spawn_cmd("sih", _task_id = px4_task_spawn_cmd("sih",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX, SCHED_PRIORITY_MAX,
1024, 1024,
(px4_main_t)&run_trampoline, (px4_main_t)&run_trampoline,
(char *const *)argv); (char *const *)argv);
if (_task_id < 0) { if (_task_id < 0) {
_task_id = -1; _task_id = -1;
return -errno; return -errno;
} }
return 0; return 0;
} }
Sih *Sih::instantiate(int argc, char *argv[]) Sih *Sih::instantiate(int argc, char *argv[])
{ {
Sih *instance = new Sih(); Sih *instance = new Sih();
if (instance == nullptr) { if (instance == nullptr) {
PX4_ERR("alloc failed"); PX4_ERR("alloc failed");
} }
return instance; return instance;
} }
Sih::Sih() Sih::Sih() :
: ModuleParams(nullptr), ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")), _loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
_sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling")) _sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
{ {
} }
void Sih::run() void Sih::run()
{ {
// to subscribe to (read) the actuators_out pwm
_actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs));
// to subscribe to (read) the actuators_out pwm // initialize parameters
_actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs)); _parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
parameters_update_poll();
// initialize parameters init_variables();
_parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); init_sensors();
parameters_update_poll();
init_variables(); const hrt_abstime task_start = hrt_absolute_time();
init_sensors(); _last_run = task_start;
_gps_time = task_start;
_serial_time = task_start;
const hrt_abstime task_start = hrt_absolute_time(); px4_sem_init(&_data_semaphore, 0, 0);
_last_run = task_start;
_gps_time = task_start;
_serial_time = task_start;
px4_sem_init(&_data_semaphore, 0, 0); hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, &_data_semaphore);
hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, &_data_semaphore); perf_begin(_sampling_perf);
perf_begin(_sampling_perf); while (!should_exit()) {
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
while (!should_exit()) perf_end(_sampling_perf);
{ perf_begin(_sampling_perf);
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
perf_end(_sampling_perf); perf_begin(_loop_perf);
perf_begin(_sampling_perf);
perf_begin(_loop_perf); inner_loop(); // main execution function
inner_loop(); // main execution function perf_end(_loop_perf);
}
perf_end(_loop_perf);
}
hrt_cancel(&_timer_call); // close the periodic timer interruption
px4_sem_destroy(&_data_semaphore);
orb_unsubscribe(_actuator_out_sub);
orb_unsubscribe(_parameter_update_sub);
hrt_cancel(&_timer_call); // close the periodic timer interruption
px4_sem_destroy(&_data_semaphore);
orb_unsubscribe(_actuator_out_sub);
orb_unsubscribe(_parameter_update_sub);
} }
// timer_callback() is used as a real time callback to post the semaphore // timer_callback() is used as a real time callback to post the semaphore
void Sih::timer_callback(void *sem) void Sih::timer_callback(void *sem)
{ {
px4_sem_post((px4_sem_t *)sem); px4_sem_post((px4_sem_t *)sem);
} }
// this is the main execution waken up periodically by the semaphore // this is the main execution waken up periodically by the semaphore
void Sih::inner_loop() void Sih::inner_loop()
{ {
_now = hrt_absolute_time(); _now = hrt_absolute_time();
_dt = (_now - _last_run) * 1e-6f; _dt = (_now - _last_run) * 1e-6f;
_last_run = _now; _last_run = _now;
read_motors(); read_motors();
generate_force_and_torques(); generate_force_and_torques();
equations_of_motion(); equations_of_motion();
reconstruct_sensors_signals(); reconstruct_sensors_signals();
send_IMU(); send_IMU();
if (_now - _gps_time >= 50000) // gps published at 20Hz if (_now - _gps_time >= 50000) { // gps published at 20Hz
{ _gps_time = _now;
_gps_time=_now; send_gps();
send_gps(); }
}
// send uart message every 40 ms // send uart message every 40 ms
if (_now - _serial_time >= 40000) if (_now - _serial_time >= 40000) {
{ _serial_time = _now;
_serial_time=_now;
publish_sih(); // publish _sih message for debug purpose publish_sih(); // publish _sih message for debug purpose
parameters_update_poll(); // update the parameters if needed parameters_update_poll(); // update the parameters if needed
} }
} }
void Sih::parameters_update_poll() void Sih::parameters_update_poll()
{ {
bool updated; bool updated = false;
struct parameter_update_s param_upd; struct parameter_update_s param_upd;
orb_check(_parameter_update_sub, &updated); orb_check(_parameter_update_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(parameter_update), _parameter_update_sub, &param_upd); orb_copy(ORB_ID(parameter_update), _parameter_update_sub, &param_upd);
updateParams(); updateParams();
parameters_updated(); parameters_updated();
} }
} }
// store the parameters in a more convenient form // store the parameters in a more convenient form
void Sih::parameters_updated() void Sih::parameters_updated()
{ {
_T_MAX = _sih_t_max.get();
_Q_MAX = _sih_q_max.get();
_L_ROLL = _sih_l_roll.get();
_L_PITCH = _sih_l_pitch.get();
_KDV = _sih_kdv.get();
_KDW = _sih_kdw.get();
_H0 = _sih_h0.get();
_T_MAX = _sih_t_max.get(); _LAT0 = (double)_sih_lat0.get() * 1.0e-7;
_Q_MAX = _sih_q_max.get(); _LON0 = (double)_sih_lon0.get() * 1.0e-7;
_L_ROLL = _sih_l_roll.get(); _COS_LAT0 = cosl(radians(_LAT0));
_L_PITCH = _sih_l_pitch.get();
_KDV = _sih_kdv.get();
_KDW = _sih_kdw.get();
_H0 = _sih_h0.get();
_LAT0 = (double)_sih_lat0.get()*1.0e-7; _MASS = _sih_mass.get();
_LON0 = (double)_sih_lon0.get()*1.0e-7;
_COS_LAT0=cosl(radians(_LAT0));
_MASS=_sih_mass.get(); _W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G);
_W_I=Vector3f(0.0f,0.0f,_MASS*CONSTANTS_ONE_G); _I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get()));
_I(0, 1) = _I(1, 0) = _sih_ixy.get();
_I(0, 2) = _I(2, 0) = _sih_ixz.get();
_I(1, 2) = _I(2, 1) = _sih_iyz.get();
_I=diag(Vector3f(_sih_ixx.get(),_sih_iyy.get(),_sih_izz.get())); _Im1 = inv(_I);
_I(0,1)=_I(1,0)=_sih_ixy.get();
_I(0,2)=_I(2,0)=_sih_ixz.get();
_I(1,2)=_I(2,1)=_sih_iyz.get();
_Im1=inv(_I);
_mu_I=Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
} }
// initialization of the variables for the simulator // initialization of the variables for the simulator
void Sih::init_variables() void Sih::init_variables()
{ {
srand(1234); // initialize the random seed once before calling generate_wgn() srand(1234); // initialize the random seed once before calling generate_wgn()
_p_I=Vector3f(0.0f,0.0f,0.0f); _p_I = Vector3f(0.0f, 0.0f, 0.0f);
_v_I=Vector3f(0.0f,0.0f,0.0f); _v_I = Vector3f(0.0f, 0.0f, 0.0f);
_q=Quatf(1.0f,0.0f,0.0f,0.0f); _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
_w_B=Vector3f(0.0f,0.0f,0.0f); _w_B = Vector3f(0.0f, 0.0f, 0.0f);
_u[0]=_u[1]=_u[2]=_u[3]=0.0f;
_u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
} }
void Sih::init_sensors() void Sih::init_sensors()
{ {
_vehicle_gps_pos.fix_type=3; // 3D fix _vehicle_gps_pos.fix_type = 3; // 3D fix
_vehicle_gps_pos.satellites_used=8; _vehicle_gps_pos.satellites_used = 8;
_vehicle_gps_pos.heading=NAN; _vehicle_gps_pos.heading = NAN;
_vehicle_gps_pos.heading_offset=NAN; _vehicle_gps_pos.heading_offset = NAN;
_vehicle_gps_pos.s_variance_m_s = 0.5f; _vehicle_gps_pos.s_variance_m_s = 0.5f;
_vehicle_gps_pos.c_variance_rad = 0.1f; _vehicle_gps_pos.c_variance_rad = 0.1f;
_vehicle_gps_pos.eph = 0.9f; _vehicle_gps_pos.eph = 0.9f;
_vehicle_gps_pos.epv = 1.78f; _vehicle_gps_pos.epv = 1.78f;
_vehicle_gps_pos.hdop = 0.7f; _vehicle_gps_pos.hdop = 0.7f;
_vehicle_gps_pos.vdop = 1.1f; _vehicle_gps_pos.vdop = 1.1f;
} }
// read the motor signals outputted from the mixer // read the motor signals outputted from the mixer
void Sih::read_motors() void Sih::read_motors()
{ {
struct actuator_outputs_s actuators_out {}; actuator_outputs_s actuators_out {};
// read the actuator outputs // read the actuator outputs
bool updated; bool updated = false;
orb_check(_actuator_out_sub, &updated); orb_check(_actuator_out_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out); orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out);
for (int i=0; i<NB_MOTORS; i++) // saturate the motor signals
_u[i]=constrain((actuators_out.output[i]-PWM_DEFAULT_MIN)/(PWM_DEFAULT_MAX-PWM_DEFAULT_MIN),0.0f, 1.0f); for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
} _u[i] = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
}
}
} }
// generate the motors thrust and torque in the body frame // generate the motors thrust and torque in the body frame
void Sih::generate_force_and_torques() void Sih::generate_force_and_torques()
{ {
_T_B=Vector3f(0.0f,0.0f,-_T_MAX*(+_u[0]+_u[1]+_u[2]+_u[3])); _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
_Mt_B=Vector3f( _L_ROLL*_T_MAX* (-_u[0]+_u[1]+_u[2]-_u[3]), _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
_L_PITCH*_T_MAX*(+_u[0]-_u[1]+_u[2]-_u[3]), _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
_Q_MAX * (+_u[0]+_u[1]-_u[2]-_u[3])); _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
_Fa_I=-_KDV*_v_I; // first order drag to slow down the aircraft _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
_Ma_B=-_KDW*_w_B; // first order angular damper _Ma_B = -_KDW * _w_B; // first order angular damper
} }
// apply the equations of motion of a rigid body and integrate one step // apply the equations of motion of a rigid body and integrate one step
void Sih::equations_of_motion() void Sih::equations_of_motion()
{ {
_C_IB=_q.to_dcm(); // body to inertial transformation _C_IB = _q.to_dcm(); // body to inertial transformation
// Equations of motion of a rigid body // Equations of motion of a rigid body
_p_I_dot=_v_I; // position differential _p_I_dot = _v_I; // position differential
_v_I_dot=(_W_I+_Fa_I+_C_IB*_T_B)/_MASS; // conservation of linear momentum _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum
_q_dot=_q.derivative1(_w_B); // attitude differential _q_dot = _q.derivative1(_w_B); // attitude differential
_w_B_dot=_Im1*(_Mt_B+_Ma_B-_w_B.cross(_I*_w_B)); // conservation of angular momentum _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum
// fake ground, avoid free fall // fake ground, avoid free fall
if(_p_I(2)>0.0f && (_v_I_dot(2)>0.0f || _v_I(2)>0.0f)) { if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
if (!_grounded) { // if we just hit the floor if (!_grounded) { // if we just hit the floor
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step // for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot=-_v_I/_dt; _v_I_dot = -_v_I / _dt;
} else {
_v_I_dot.setZero(); } else {
} _v_I_dot.setZero();
_v_I.setZero(); }
_w_B.setZero();
_grounded = true; _v_I.setZero();
} else { _w_B.setZero();
// integration: Euler forward _grounded = true;
_p_I = _p_I + _p_I_dot*_dt;
_v_I = _v_I + _v_I_dot*_dt; } else {
_q = _q+_q_dot*_dt; // as given in attitude_estimator_q_main.cpp // integration: Euler forward
_q.normalize(); _p_I = _p_I + _p_I_dot * _dt;
_w_B = _w_B + _w_B_dot*_dt; _v_I = _v_I + _v_I_dot * _dt;
_grounded = false; _q = _q + _q_dot * _dt; // as given in attitude_estimator_q_main.cpp
} _q.normalize();
_w_B = _w_B + _w_B_dot * _dt;
_grounded = false;
}
} }
// reconstruct the noisy sensor signals // reconstruct the noisy sensor signals
void Sih::reconstruct_sensors_signals() void Sih::reconstruct_sensors_signals()
{ {
// The sensor signals reconstruction and noise levels are from
// Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
// The sensor signals reconstruction and noise levels are from // IMU
// Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." _acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. _gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
_mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
// IMU // barometer
_acc=_C_IB.transpose()*(_v_I_dot-Vector3f(0.0f,0.0f,CONSTANTS_ONE_G))+noiseGauss3f(0.5f,1.7f,1.4f); float altitude = (_H0 - _p_I(2)) + generate_wgn() * 0.14f; // altitude with noise
_gyro=_w_B+noiseGauss3f(0.14f,0.07f,0.03f); _baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar
_mag=_C_IB.transpose()*_mu_I+noiseGauss3f(0.02f,0.02f,0.03f); powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST));
_baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in celcius
// barometer // GPS
float altitude=(_H0-_p_I(2))+generate_wgn()*0.14f; // altitude with noise _gps_lat_noiseless = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH);
_baro_p_mBar=CONSTANTS_STD_PRESSURE_MBAR* // reconstructed pressure in mBar _gps_lon_noiseless = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;
powf((1.0f+altitude*TEMP_GRADIENT/T1_K),-CONSTANTS_ONE_G/(TEMP_GRADIENT*CONSTANTS_AIR_GAS_CONST)); _gps_alt_noiseless = _H0 - _p_I(2);
_baro_temp_c=T1_K+CONSTANTS_ABSOLUTE_NULL_CELSIUS+TEMP_GRADIENT*altitude; // reconstructed temperture in celcius
// GPS _gps_lat = _gps_lat_noiseless + (double)(generate_wgn() * 7.2e-6f); // latitude in degrees
_gps_lat_noiseless=_LAT0+degrees((double)_p_I(0)/CONSTANTS_RADIUS_OF_EARTH); _gps_lon = _gps_lon_noiseless + (double)(generate_wgn() * 1.75e-5f); // longitude in degrees
_gps_lon_noiseless=_LON0+degrees((double)_p_I(1)/CONSTANTS_RADIUS_OF_EARTH)/_COS_LAT0; _gps_alt = _gps_alt_noiseless + generate_wgn() * 1.78f;
_gps_alt_noiseless=_H0-_p_I(2); _gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f);
_gps_lat=_gps_lat_noiseless+(double)(generate_wgn()*7.2e-6f); // latitude in degrees
_gps_lon=_gps_lon_noiseless+(double)(generate_wgn()*1.75e-5f); // longitude in degrees
_gps_alt=_gps_alt_noiseless+generate_wgn()*1.78f;
_gps_vel=_v_I+noiseGauss3f(0.06f,0.077f,0.158f);
} }
void Sih::send_IMU() void Sih::send_IMU()
@@ -422,93 +384,133 @@ void Sih::send_IMU()
void Sih::send_gps() void Sih::send_gps()
{ {
_vehicle_gps_pos.timestamp=_now; _vehicle_gps_pos.timestamp = _now;
_vehicle_gps_pos.lat=(int32_t)(_gps_lat*1e7); // Latitude in 1E-7 degrees _vehicle_gps_pos.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees
_vehicle_gps_pos.lon=(int32_t)(_gps_lon*1e7); // Longitude in 1E-7 degrees _vehicle_gps_pos.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees
_vehicle_gps_pos.alt=(int32_t)(_gps_alt*1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres) _vehicle_gps_pos.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres)
_vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt*1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres) _vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
_vehicle_gps_pos.vel_ned_valid=true; // True if NED velocity is valid _vehicle_gps_pos.vel_ned_valid = true; // True if NED velocity is valid
_vehicle_gps_pos.vel_m_s=sqrtf(_gps_vel(0)*_gps_vel(0)+_gps_vel(1)*_gps_vel(1)); // GPS ground speed, (metres/sec) _vehicle_gps_pos.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel(
_vehicle_gps_pos.vel_n_m_s=_gps_vel(0); // GPS North velocity, (metres/sec) 1)); // GPS ground speed, (metres/sec)
_vehicle_gps_pos.vel_e_m_s=_gps_vel(1); // GPS East velocity, (metres/sec) _vehicle_gps_pos.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec)
_vehicle_gps_pos.vel_d_m_s=_gps_vel(2); // GPS Down velocity, (metres/sec) _vehicle_gps_pos.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec)
_vehicle_gps_pos.cog_rad=atan2(_gps_vel(1),_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) _vehicle_gps_pos.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec)
if (_vehicle_gps_pos_pub != nullptr) { _vehicle_gps_pos.cog_rad = atan2(_gps_vel(1),
orb_publish(ORB_ID(vehicle_gps_position), _vehicle_gps_pos_pub, &_vehicle_gps_pos); _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
} else {
_vehicle_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_vehicle_gps_pos); if (_vehicle_gps_pos_pub != nullptr) {
} orb_publish(ORB_ID(vehicle_gps_position), _vehicle_gps_pos_pub, &_vehicle_gps_pos);
} else {
_vehicle_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_vehicle_gps_pos);
}
} }
void Sih::publish_sih() void Sih::publish_sih()
{ {
_gpos_gt.timestamp = hrt_absolute_time();
_gpos_gt.lat = _gps_lat_noiseless;
_gpos_gt.lon = _gps_lon_noiseless;
_gpos_gt.alt = _gps_alt_noiseless;
_gpos_gt.vel_n = _v_I(0);
_gpos_gt.vel_e = _v_I(1);
_gpos_gt.vel_d = _v_I(2);
_gpos_gt.timestamp=hrt_absolute_time(); if (_gpos_gt_sub != nullptr) {
_gpos_gt.lat=_gps_lat_noiseless; orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt);
_gpos_gt.lon=_gps_lon_noiseless;
_gpos_gt.alt=_gps_alt_noiseless;
_gpos_gt.vel_n=_v_I(0);
_gpos_gt.vel_e=_v_I(1);
_gpos_gt.vel_d=_v_I(2);
if (_gpos_gt_sub != nullptr) { } else {
orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt); _gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
} else { }
_gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
}
// publish attitude groundtruth // publish attitude groundtruth
_att_gt.timestamp=hrt_absolute_time(); _att_gt.timestamp = hrt_absolute_time();
_att_gt.q[0]=_q(0); _att_gt.q[0] = _q(0);
_att_gt.q[1]=_q(1); _att_gt.q[1] = _q(1);
_att_gt.q[2]=_q(2); _att_gt.q[2] = _q(2);
_att_gt.q[3]=_q(3); _att_gt.q[3] = _q(3);
_att_gt.rollspeed=_w_B(0); _att_gt.rollspeed = _w_B(0);
_att_gt.pitchspeed=_w_B(1); _att_gt.pitchspeed = _w_B(1);
_att_gt.yawspeed=_w_B(2); _att_gt.yawspeed = _w_B(2);
if (_att_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt); if (_att_gt_sub != nullptr) {
} else { orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt);
_att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
} } else {
_att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
}
} }
float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
{ {
// algorithm 1: // algorithm 1:
// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
// algorithm 2: from BlockRandGauss.hpp // algorithm 2: from BlockRandGauss.hpp
static float V1, V2, S; static float V1, V2, S;
static bool phase = true; static bool phase = true;
float X; float X;
if (phase) { if (phase) {
do { do {
float U1 = (float)rand() / RAND_MAX; float U1 = (float)rand() / RAND_MAX;
float U2 = (float)rand() / RAND_MAX; float U2 = (float)rand() / RAND_MAX;
V1 = 2.0f * U1 - 1.0f; V1 = 2.0f * U1 - 1.0f;
V2 = 2.0f * U2 - 1.0f; V2 = 2.0f * U2 - 1.0f;
S = V1 * V1 + V2 * V2; S = V1 * V1 + V2 * V2;
} while (S >= 1.0f || fabsf(S) < 1e-8f); } while (S >= 1.0f || fabsf(S) < 1e-8f);
X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
} else { } else {
X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
} }
phase = !phase; phase = !phase;
return X; return X;
} }
// generate white Gaussian noise sample vector with specified std // generate white Gaussian noise sample vector with specified std
Vector3f Sih::noiseGauss3f(float stdx,float stdy, float stdz) Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
{ {
return Vector3f(generate_wgn()*stdx,generate_wgn()*stdy,generate_wgn()*stdz); return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
} }
int sih_main(int argc, char *argv[]) int sih_main(int argc, char *argv[])
{ {
return Sih::main(argc, argv); return Sih::main(argc, argv);
}
int Sih::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module provide a simulator for quadrotors running fully
inside the hardware autopilot.
This simulator subscribes to "actuator_outputs" which are the actuator pwm
signals given by the mixer.
This simulator publishes the sensors signals corrupted with realistic noise
in order to incorporate the state estimator in the loop.
### Implementation
The simulator implements the equations of motion using matrix algebra.
Quaternion representation is used for the attitude.
Forward Euler is used for integration.
Most of the variables are declared global in the .hpp file to avoid stack overflow.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("sih", "simulation");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
} }