zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sih: fix code style

sbg
Daniel Agar 6 years ago
parent
09eaef82f6
commit
be99d2f111
1 changed files with 295 additions and 293 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 588
      src/modules/sih/sih.cpp

588
src/modules/sih/sih.cpp
Unescape View File

@ -57,334 +57,296 @@ @@ -57,334 +57,296 @@
using namespace math;
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()
{
PX4_INFO("Running");
return 0;
PX4_INFO("Running");
return 0;
}
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[])
{
_task_id = px4_task_spawn_cmd("sih",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1024,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
_task_id = px4_task_spawn_cmd("sih",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1024,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
return 0;
}
Sih *Sih::instantiate(int argc, char *argv[])
{
Sih *instance = new Sih();
Sih *instance = new Sih();
if (instance == nullptr) {
PX4_ERR("alloc failed");
}
if (instance == nullptr) {
PX4_ERR("alloc failed");
}
return instance;
return instance;
}
Sih::Sih()
: ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
_sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
Sih::Sih() :
ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
_sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
{
}
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
_actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs));
// initialize parameters
_parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
parameters_update_poll();
init_variables();
init_sensors();
// initialize parameters
_parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
parameters_update_poll();
const hrt_abstime task_start = hrt_absolute_time();
_last_run = task_start;
_gps_time = task_start;
_serial_time = task_start;
init_variables();
init_sensors();
px4_sem_init(&_data_semaphore, 0, 0);
const hrt_abstime task_start = hrt_absolute_time();
_last_run = task_start;
_gps_time = task_start;
_serial_time = task_start;
hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, &_data_semaphore);
px4_sem_init(&_data_semaphore, 0, 0);
perf_begin(_sampling_perf);
hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, &_data_semaphore);
while (!should_exit())
{
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
perf_begin(_sampling_perf);
perf_end(_sampling_perf);
perf_begin(_sampling_perf);
while (!should_exit()) {
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
perf_begin(_loop_perf);
perf_end(_sampling_perf);
perf_begin(_sampling_perf);
inner_loop(); // main execution function
perf_begin(_loop_perf);
perf_end(_loop_perf);
}
inner_loop(); // main execution function
hrt_cancel(&_timer_call); // close the periodic timer interruption
px4_sem_destroy(&_data_semaphore);
orb_unsubscribe(_actuator_out_sub);
orb_unsubscribe(_parameter_update_sub);
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);
}
// timer_callback() is used as a real time callback to post the semaphore
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
void Sih::inner_loop()
{
_now = hrt_absolute_time();
_dt = (_now - _last_run) * 1e-6f;
_last_run = _now;
_now = hrt_absolute_time();
_dt = (_now - _last_run) * 1e-6f;
_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
{
_gps_time=_now;
send_gps();
}
if (_now - _gps_time >= 50000) { // gps published at 20Hz
_gps_time = _now;
send_gps();
}
// send uart message every 40 ms
if (_now - _serial_time >= 40000)
{
_serial_time=_now;
// send uart message every 40 ms
if (_now - _serial_time >= 40000) {
_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()
{
bool updated;
struct parameter_update_s param_upd;
bool updated = false;
struct parameter_update_s param_upd;
orb_check(_parameter_update_sub, &updated);
orb_check(_parameter_update_sub, &updated);
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameter_update_sub, &param_upd);
updateParams();
parameters_updated();
}
if (updated) {
orb_copy(ORB_ID(parameter_update), _parameter_update_sub, &param_upd);
updateParams();
parameters_updated();
}
}
// store the parameters in a more convenient form
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();
_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();
_LAT0 = (double)_sih_lat0.get()*1.0e-7;
_LON0 = (double)_sih_lon0.get()*1.0e-7;
_COS_LAT0=cosl(radians(_LAT0));
_LAT0 = (double)_sih_lat0.get() * 1.0e-7;
_LON0 = (double)_sih_lon0.get() * 1.0e-7;
_COS_LAT0 = cosl(radians(_LAT0));
_MASS=_sih_mass.get();
_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()));
_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());
_Im1 = inv(_I);
_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
}
// initialization of the variables for the simulator
void Sih::init_variables()
{
srand(1234); // initialize the random seed once before calling generate_wgn()
_p_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);
_w_B=Vector3f(0.0f,0.0f,0.0f);
srand(1234); // initialize the random seed once before calling generate_wgn()
_u[0]=_u[1]=_u[2]=_u[3]=0.0f;
_p_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);
_w_B = Vector3f(0.0f, 0.0f, 0.0f);
_u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
}
void Sih::init_sensors()
{
_vehicle_gps_pos.fix_type=3; // 3D fix
_vehicle_gps_pos.satellites_used=8;
_vehicle_gps_pos.heading=NAN;
_vehicle_gps_pos.heading_offset=NAN;
_vehicle_gps_pos.s_variance_m_s = 0.5f;
_vehicle_gps_pos.c_variance_rad = 0.1f;
_vehicle_gps_pos.eph = 0.9f;
_vehicle_gps_pos.epv = 1.78f;
_vehicle_gps_pos.hdop = 0.7f;
_vehicle_gps_pos.vdop = 1.1f;
_vehicle_gps_pos.fix_type = 3; // 3D fix
_vehicle_gps_pos.satellites_used = 8;
_vehicle_gps_pos.heading = NAN;
_vehicle_gps_pos.heading_offset = NAN;
_vehicle_gps_pos.s_variance_m_s = 0.5f;
_vehicle_gps_pos.c_variance_rad = 0.1f;
_vehicle_gps_pos.eph = 0.9f;
_vehicle_gps_pos.epv = 1.78f;
_vehicle_gps_pos.hdop = 0.7f;
_vehicle_gps_pos.vdop = 1.1f;
}
// read the motor signals outputted from the mixer
void Sih::read_motors()
{
struct actuator_outputs_s actuators_out {};
actuator_outputs_s actuators_out {};
// read the actuator outputs
bool updated = false;
orb_check(_actuator_out_sub, &updated);
// read the actuator outputs
bool updated;
orb_check(_actuator_out_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out);
if (updated) {
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
void Sih::generate_force_and_torques()
{
_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]),
_L_PITCH*_T_MAX*(+_u[0]-_u[1]+_u[2]-_u[3]),
_Q_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]),
_L_PITCH * _T_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
_Ma_B=-_KDW*_w_B; // first order angular damper
_Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
_Ma_B = -_KDW * _w_B; // first order angular damper
}
// apply the equations of motion of a rigid body and integrate one step
void Sih::equations_of_motion()
{
_C_IB=_q.to_dcm(); // body to inertial transformation
// Equations of motion of a rigid body
_p_I_dot=_v_I; // position differential
_v_I_dot=(_W_I+_Fa_I+_C_IB*_T_B)/_MASS; // conservation of linear momentum
_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
// fake ground, avoid free fall
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
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot=-_v_I/_dt;
} else {
_v_I_dot.setZero();
}
_v_I.setZero();
_w_B.setZero();
_grounded = true;
} else {
// integration: Euler forward
_p_I = _p_I + _p_I_dot*_dt;
_v_I = _v_I + _v_I_dot*_dt;
_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;
}
_C_IB = _q.to_dcm(); // body to inertial transformation
// Equations of motion of a rigid body
_p_I_dot = _v_I; // position differential
_v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum
_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
// fake ground, avoid free fall
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
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot = -_v_I / _dt;
} else {
_v_I_dot.setZero();
}
_v_I.setZero();
_w_B.setZero();
_grounded = true;
} else {
// integration: Euler forward
_p_I = _p_I + _p_I_dot * _dt;
_v_I = _v_I + _v_I_dot * _dt;
_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
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.
// IMU
_acc=_C_IB.transpose()*(_v_I_dot-Vector3f(0.0f,0.0f,CONSTANTS_ONE_G))+noiseGauss3f(0.5f,1.7f,1.4f);
_gyro=_w_B+noiseGauss3f(0.14f,0.07f,0.03f);
_mag=_C_IB.transpose()*_mu_I+noiseGauss3f(0.02f,0.02f,0.03f);
// barometer
float altitude=(_H0-_p_I(2))+generate_wgn()*0.14f; // altitude with noise
_baro_p_mBar=CONSTANTS_STD_PRESSURE_MBAR* // reconstructed pressure in mBar
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
// GPS
_gps_lat_noiseless=_LAT0+degrees((double)_p_I(0)/CONSTANTS_RADIUS_OF_EARTH);
_gps_lon_noiseless=_LON0+degrees((double)_p_I(1)/CONSTANTS_RADIUS_OF_EARTH)/_COS_LAT0;
_gps_alt_noiseless=_H0-_p_I(2);
_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);
// 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.
// IMU
_acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
_gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
_mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
// barometer
float altitude = (_H0 - _p_I(2)) + generate_wgn() * 0.14f; // altitude with noise
_baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar
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
// GPS
_gps_lat_noiseless = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH);
_gps_lon_noiseless = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;
_gps_alt_noiseless = _H0 - _p_I(2);
_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()
@ -422,93 +384,133 @@ void Sih::send_IMU() @@ -422,93 +384,133 @@ void Sih::send_IMU()
void Sih::send_gps()
{
_vehicle_gps_pos.timestamp=_now;
_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.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.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_n_m_s=_gps_vel(0); // GPS North velocity, (metres/sec)
_vehicle_gps_pos.vel_e_m_s=_gps_vel(1); // GPS East velocity, (metres/sec)
_vehicle_gps_pos.vel_d_m_s=_gps_vel(2); // GPS Down 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)
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);
}
_vehicle_gps_pos.timestamp = _now;
_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.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.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_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec)
_vehicle_gps_pos.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec)
_vehicle_gps_pos.vel_d_m_s = _gps_vel(2); // GPS Down 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)
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()
{
_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);
if (_gpos_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt);
} else {
_gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
}
_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);
if (_gpos_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt);
} else {
_gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
}
// publish attitude groundtruth
_att_gt.timestamp=hrt_absolute_time();
_att_gt.q[0]=_q(0);
_att_gt.q[1]=_q(1);
_att_gt.q[2]=_q(2);
_att_gt.q[3]=_q(3);
_att_gt.rollspeed=_w_B(0);
_att_gt.pitchspeed=_w_B(1);
_att_gt.yawspeed=_w_B(2);
if (_att_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt);
} else {
_att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
}
// publish attitude groundtruth
_att_gt.timestamp = hrt_absolute_time();
_att_gt.q[0] = _q(0);
_att_gt.q[1] = _q(1);
_att_gt.q[2] = _q(2);
_att_gt.q[3] = _q(3);
_att_gt.rollspeed = _w_B(0);
_att_gt.pitchspeed = _w_B(1);
_att_gt.yawspeed = _w_B(2);
if (_att_gt_sub != nullptr) {
orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_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
{
// algorithm 1:
// 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);
// algorithm 2: from BlockRandGauss.hpp
static float V1, V2, S;
static bool phase = true;
float X;
if (phase) {
do {
float U1 = (float)rand() / RAND_MAX;
float U2 = (float)rand() / RAND_MAX;
V1 = 2.0f * U1 - 1.0f;
V2 = 2.0f * U2 - 1.0f;
S = V1 * V1 + V2 * V2;
} while (S >= 1.0f || fabsf(S) < 1e-8f);
X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
} else {
X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
}
phase = !phase;
return X;
// algorithm 1:
// 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);
// algorithm 2: from BlockRandGauss.hpp
static float V1, V2, S;
static bool phase = true;
float X;
if (phase) {
do {
float U1 = (float)rand() / RAND_MAX;
float U2 = (float)rand() / RAND_MAX;
V1 = 2.0f * U1 - 1.0f;
V2 = 2.0f * U2 - 1.0f;
S = V1 * V1 + V2 * V2;
} while (S >= 1.0f || fabsf(S) < 1e-8f);
X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
} else {
X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
}
phase = !phase;
return X;
}
// 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[])
{
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;
}

Write Preview
Loading…
Cancel
Save