fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 12:54:07 +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-15 12:28:54 -05:00
parent 27e9b1e3d4
commit 4fef3dd7d5
5 changed files with 102 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mavlink/CMakeLists.txt
View File

@ -63,6 +63,10 @@ px4_add_module(
module.yaml
DEPENDS
airspeed
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
git_mavlink_v2
conversion
git_ecl

119
src/modules/mavlink/mavlink_receiver.cpp
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 */

21
src/modules/mavlink/mavlink_receiver.h
View File

@ -47,6 +47,10 @@
#include "mavlink_parameters.h"
#include "mavlink_timesync.h"
#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 <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
@ -79,11 +83,6 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_attitude.h>
@ -107,7 +106,7 @@ class MavlinkReceiver : public ModuleParams
{
public:
MavlinkReceiver(Mavlink *parent);
~MavlinkReceiver() = default;
~MavlinkReceiver() override;
/**
* Start the receiver thread
@ -258,10 +257,6 @@ private:
uORB::PublicationMulti<manual_control_setpoint_s> _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
uORB::PublicationMulti<ping_s> _ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
uORB::PublicationMulti<sensor_accel_s> _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW};
uORB::PublicationMulti<sensor_baro_s> _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW};
uORB::PublicationMulti<sensor_gyro_s> _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW};
uORB::PublicationMulti<sensor_mag_s> _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW};
// ORB publications (queue length > 1)
uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
@ -277,6 +272,12 @@ private:
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
// hil_sensor and hil_state_quaternion
PX4Accelerometer *_px4_accel{nullptr};
PX4Barometer *_px4_baro{nullptr};
PX4Gyroscope *_px4_gyro{nullptr};
PX4Magnetometer *_px4_mag{nullptr};
static constexpr unsigned int MOM_SWITCH_COUNT{8};
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
uint16_t _mom_switch_state{0};

34
src/modules/sih/sih.cpp
View File

@ -92,8 +92,8 @@ Sih *Sih::instantiate(int argc, char *argv[])
Sih::Sih() :
ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
_sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")),
_sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling"))
{
}
@ -333,34 +333,20 @@ void Sih::reconstruct_sensors_signals()
void Sih::send_IMU()
{
// 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);
}
_px4_gyro.set_temperature(T1_C);
_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
// 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);
}
_px4_accel.set_temperature(T1_C);
_px4_accel.update(_now, _acc(0), _acc(1), _acc(2));
// 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);
}
_px4_mag.set_temperature(T1_C);
_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
// baro
{
_px4_baro.set_temperature(_baro_temp_c);
_px4_baro.update(_now, _baro_p_mBar);
}
_px4_baro.set_temperature(_baro_temp_c);
_px4_baro.update(_now, _baro_p_mBar);
}
void Sih::send_gps()

12
src/modules/simulator/simulator_mavlink.cpp
View File

@ -195,26 +195,20 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
// gyro
if (!_param_sim_gyro_block.get()) {
static constexpr float scaling = 1000.0f;
_px4_gyro.set_scale(1 / scaling);
_px4_gyro.set_temperature(imu.temperature);
_px4_gyro.update(time, imu.xgyro * scaling, imu.ygyro * scaling, imu.zgyro * scaling);
_px4_gyro.update(time, imu.xgyro, imu.ygyro, imu.zgyro);
}
// accel
if (!_param_sim_accel_block.get()) {
static constexpr float scaling = 1000.0f;
_px4_accel.set_scale(1 / scaling);
_px4_accel.set_temperature(imu.temperature);
_px4_accel.update(time, imu.xacc * scaling, imu.yacc * scaling, imu.zacc * scaling);
_px4_accel.update(time, imu.xacc, imu.yacc, imu.zacc);
}
// magnetometer
if (!_param_sim_mag_block.get()) {
static constexpr float scaling = 1000.0f;
_px4_mag.set_scale(1 / scaling);
_px4_mag.set_temperature(imu.temperature);
_px4_mag.update(time, imu.xmag * scaling, imu.ymag * scaling, imu.zmag * scaling);
_px4_mag.update(time, imu.xmag, imu.ymag, imu.zmag);
}
// baro