Skip to content

Commit

Permalink
sih: move to PX4Accelerometer/PX4Gyroscope/PX4Magnetometer/PX4Baromet…
Browse files Browse the repository at this point in the history 
…er helpers
  • Loading branch information
@dagar
dagar committed Jul 28, 2019
1 parent f432f74 commit 44a43ee
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 77 deletions.
4 changes: 4 additions & 0 deletions src/modules/sih/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,4 +41,8 @@ px4_add_module(
sih.cpp
DEPENDS
mathlib
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
)
86 changes: 28 additions & 58 deletions src/modules/sih/sih.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include <px4_log.h>

#include <drivers/drv_pwm_output.h> // to get PWM flags
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h> // to get the HIL status

#include <unistd.h>
Expand Down Expand Up @@ -283,28 +284,6 @@ void Sih::init_variables()

void Sih::init_sensors()
{

_sensor_accel.device_id=1;
_sensor_accel.error_count=0;
_sensor_accel.integral_dt=0;
_sensor_accel.temperature=T1_C;
_sensor_accel.scaling=0.0f;

_sensor_gyro.device_id=1;
_sensor_gyro.error_count=0;
_sensor_gyro.integral_dt=0;
_sensor_gyro.temperature=T1_C;
_sensor_gyro.scaling=0.0f;

_sensor_mag.device_id=1;
_sensor_mag.error_count=0;
_sensor_mag.temperature=T1_C;
_sensor_mag.scaling=0.0f;
_sensor_mag.is_external=false;

_sensor_baro.error_count=0;
_sensor_baro.device_id=1;

_vehicle_gps_pos.fix_type=3; // 3D fix
_vehicle_gps_pos.satellites_used=8;
_vehicle_gps_pos.heading=NAN;
Expand Down Expand Up @@ -410,44 +389,35 @@ void Sih::reconstruct_sensors_signals()

void Sih::send_IMU()
{
_sensor_accel.timestamp=_now;
_sensor_accel.x=_acc(0);
_sensor_accel.y=_acc(1);
_sensor_accel.z=_acc(2);
if (_sensor_accel_pub != nullptr) {
orb_publish(ORB_ID(sensor_accel), _sensor_accel_pub, &_sensor_accel);
} else {
_sensor_accel_pub = orb_advertise(ORB_ID(sensor_accel), &_sensor_accel);
}
// gyro
{
static constexpr float scaling = 1000.0f;
_px4_gyro.set_scale(1 / scaling);
_px4_gyro.set_temperature(T1_C);
_px4_gyro.update(_now, _gyro(0) * scaling, _gyro(1) * scaling, _gyro(2) * scaling);
}

_sensor_gyro.timestamp=_now;
_sensor_gyro.x=_gyro(0);
_sensor_gyro.y=_gyro(1);
_sensor_gyro.z=_gyro(2);
if (_sensor_gyro_pub != nullptr) {
orb_publish(ORB_ID(sensor_gyro), _sensor_gyro_pub, &_sensor_gyro);
} else {
_sensor_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &_sensor_gyro);
}
// accel
{
static constexpr float scaling = 1000.0f;
_px4_accel.set_scale(1 / scaling);
_px4_accel.set_temperature(T1_C);
_px4_accel.update(_now, _acc(0) * scaling, _acc(1) * scaling, _acc(2) * scaling);
}

_sensor_mag.timestamp=_now;
_sensor_mag.x=_mag(0);
_sensor_mag.y=_mag(1);
_sensor_mag.z=_mag(2);
if (_sensor_mag_pub != nullptr) {
orb_publish(ORB_ID(sensor_mag), _sensor_mag_pub, &_sensor_mag);
} else {
_sensor_mag_pub = orb_advertise(ORB_ID(sensor_mag), &_sensor_mag);
}
// magnetometer
{
static constexpr float scaling = 1000.0f;
_px4_mag.set_scale(1 / scaling);
_px4_mag.set_temperature(T1_C);
_px4_mag.update(_now, _mag(0) * scaling, _mag(1) * scaling, _mag(2) * scaling);
}

_sensor_baro.timestamp=_now;
_sensor_baro.pressure=_baro_p_mBar;
_sensor_baro.temperature=_baro_temp_c;
if (_sensor_baro_pub != nullptr) {
orb_publish(ORB_ID(sensor_baro), _sensor_baro_pub, &_sensor_baro);
} else {
_sensor_baro_pub = orb_advertise(ORB_ID(sensor_baro), &_sensor_baro);
}
// baro
{
_px4_baro.set_temperature(_baro_temp_c);
_px4_baro.update(_now, _baro_p_mBar);
}
}

void Sih::send_gps()
Expand Down Expand Up @@ -501,7 +471,7 @@ void Sih::publish_sih()
} 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
{
Expand Down
30 changes: 11 additions & 19 deletions src/modules/sih/sih.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,17 +41,15 @@
#include <conversion/rotation.h> // math::radians,
#include <ecl/geo/geo.h> // to get the physical constants
#include <drivers/drv_hrt.h> // to get the real time
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <perf/perf_counter.h>

#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
#include <uORB/topics/vehicle_gps_position.h>

extern "C" __EXPORT int sih_main(int argc, char *argv[]);

Expand Down Expand Up @@ -98,18 +96,12 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
void parameters_update_poll();
void parameters_updated();

// to publish the sensor baro
struct sensor_baro_s _sensor_baro {};
orb_advert_t _sensor_baro_pub{nullptr};
// to publish the sensor mag
struct sensor_mag_s _sensor_mag {};
orb_advert_t _sensor_mag_pub{nullptr};
// to publish the sensor gyroscope
struct sensor_gyro_s _sensor_gyro {};
orb_advert_t _sensor_gyro_pub{nullptr};
// to publish the sensor accelerometer
struct sensor_accel_s _sensor_accel {};
orb_advert_t _sensor_accel_pub{nullptr};
// simulated sensor instances
PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{ 2294028, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
PX4Magnetometer _px4_mag{ 197388, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION

// to publish the gps position
struct vehicle_gps_position_s _vehicle_gps_pos {};
orb_advert_t _vehicle_gps_pos_pub{nullptr};
Expand Down

0 comments on commit 44a43ee

Please sign in to comment.