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

simulated sensors minor cleanup

Browse Source 
- mavlink receiver use PX4Accelerometer/PX4Gyroscope for hil_sensor and hil_state_quaternion
 - simulator module remove unnecessary fake scaling
 - sih module remove unnecessary fake scaling
This commit is contained in:
Daniel Agar
2020-01-24 12:50:22 -05:00
parent 27e9b1e3d4
commit 4fef3dd7d5
5 changed files with 102 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+74 -45
View File
@@ -80,6 +80,14 @@
using matrix::wrap_2pi;
MavlinkReceiver::~MavlinkReceiver()
{
delete _px4_accel;
delete _px4_baro;
delete _px4_gyro;
delete _px4_mag;
}
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
ModuleParams(nullptr),
_mavlink(parent),
@@ -2085,57 +2093,70 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
/* gyro */
{
sensor_gyro_s gyro{};
if (_px4_gyro == nullptr) {
// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(2294028);
gyro.timestamp = timestamp;
gyro.x = imu.xgyro;
gyro.y = imu.ygyro;
gyro.z = imu.zgyro;
gyro.temperature = imu.temperature;
if (_px4_gyro == nullptr) {
PX4_ERR("PX4Gyroscope alloc failed");
}
}
_gyro_pub.publish(gyro);
if (_px4_gyro != nullptr) {
_px4_gyro->set_temperature(imu.temperature);
_px4_gyro->update(timestamp, imu.xgyro, imu.ygyro, imu.zgyro);
}
}
/* accelerometer */
{
sensor_accel_s accel{};
if (_px4_accel == nullptr) {
// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
accel.timestamp = timestamp;
accel.x = imu.xacc;
accel.y = imu.yacc;
accel.z = imu.zacc;
accel.temperature = imu.temperature;
if (_px4_accel == nullptr) {
PX4_ERR("PX4Accelerometer alloc failed");
}
}
_accel_pub.publish(accel);
if (_px4_accel != nullptr) {
_px4_accel->set_temperature(imu.temperature);
_px4_accel->update(timestamp, imu.xacc, imu.yacc, imu.zacc);
}
}
/* magnetometer */
{
sensor_mag_s mag{};
if (_px4_mag == nullptr) {
// 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
_px4_mag = new PX4Magnetometer(197388);
mag.timestamp = timestamp;
mag.x_raw = imu.xmag * 1000.0f;
mag.y_raw = imu.ymag * 1000.0f;
mag.z_raw = imu.zmag * 1000.0f;
mag.x = imu.xmag;
mag.y = imu.ymag;
mag.z = imu.zmag;
if (_px4_mag == nullptr) {
PX4_ERR("PX4Magnetometer alloc failed");
}
}
_mag_pub.publish(mag);
if (_px4_mag != nullptr) {
_px4_mag->set_temperature(imu.temperature);
_px4_mag->update(timestamp, imu.xmag, imu.ymag, imu.zmag);
}
}
/* baro */
{
sensor_baro_s baro{};
if (_px4_baro == nullptr) {
// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
_px4_baro = new PX4Barometer(6620172);
baro.timestamp = timestamp;
baro.pressure = imu.abs_pressure;
baro.temperature = imu.temperature;
if (_px4_baro == nullptr) {
PX4_ERR("PX4Barometer alloc failed");
}
}
/* fake device ID */
baro.device_id = 1234567;
_baro_pub.publish(baro);
if (_px4_baro != nullptr) {
_px4_baro->set_temperature(imu.temperature);
_px4_baro->update(timestamp, imu.abs_pressure);
}
}
/* battery status */
@@ -2498,28 +2519,36 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* accelerometer */
{
sensor_accel_s accel{};
if (_px4_accel == nullptr) {
// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
accel.timestamp = timestamp;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
accel.temperature = 25.0f;
if (_px4_accel == nullptr) {
PX4_ERR("PX4Accelerometer alloc failed");
}
}
_accel_pub.publish(accel);
if (_px4_accel != nullptr) {
// accel in mG
_px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f);
_px4_accel->update(timestamp, hil_state.xacc, hil_state.yacc, hil_state.zacc);
}
}
/* gyroscope */
{
sensor_gyro_s gyro{};
if (_px4_gyro == nullptr) {
// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(2294028);
gyro.timestamp = timestamp;
gyro.x = hil_state.rollspeed;
gyro.y = hil_state.pitchspeed;
gyro.z = hil_state.yawspeed;
gyro.temperature = 25.0f;
if (_px4_gyro == nullptr) {
PX4_ERR("PX4Gyroscope alloc failed");
}
}
_gyro_pub.publish(gyro);
if (_px4_gyro != nullptr) {
_px4_gyro->update(timestamp, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
}
}
/* battery status */