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

move baro and magnetometer data out of sensor_combined

Browse Source
This commit is contained in:
Daniel Agar
2018-04-11 23:26:58 -04:00
parent 3e6ba1d541
commit 3b5b12e1d1
31 changed files with 289 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+44 -32
View File
@@ -104,6 +104,8 @@
#include <uORB/topics/collision_report.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/uORB.h>
static uint16_t cm_uint16_from_m_float(float m);
@@ -657,6 +659,8 @@ private:
MavlinkOrbSubscription *_bias_sub;
MavlinkOrbSubscription *_differential_pressure_sub;
MavlinkOrbSubscription *_magnetometer_sub;
MavlinkOrbSubscription *_air_data_sub;
uint64_t _accel_timestamp;
uint64_t _gyro_timestamp;
@@ -673,6 +677,8 @@ protected:
_sensor_time(0),
_bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))),
_differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))),
_magnetometer_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_magnetometer))),
_air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))),
_accel_timestamp(0),
_gyro_timestamp(0),
_mag_timestamp(0),
@@ -698,20 +704,27 @@ protected:
_gyro_timestamp = sensor.timestamp;
}
if (_mag_timestamp != sensor.timestamp + sensor.magnetometer_timestamp_relative) {
vehicle_magnetometer_s magnetometer = {};
_magnetometer_sub->update(&magnetometer);
if (_mag_timestamp != magnetometer.timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
_mag_timestamp = sensor.timestamp + sensor.magnetometer_timestamp_relative;
_mag_timestamp = magnetometer.timestamp;
}
if (_baro_timestamp != sensor.timestamp + sensor.baro_timestamp_relative) {
vehicle_air_data_s air_data = {};
_air_data_sub->update(&air_data);
if (_baro_timestamp != air_data.timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
_baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
_baro_timestamp = air_data.timestamp;
}
sensor_bias_s bias = {};
_bias_sub->update(&bias);
differential_pressure_s differential_pressure = {};
_differential_pressure_sub->update(&differential_pressure);
@@ -724,13 +737,13 @@ protected:
msg.xgyro = sensor.gyro_rad[0] - bias.gyro_x_bias;
msg.ygyro = sensor.gyro_rad[1] - bias.gyro_y_bias;
msg.zgyro = sensor.gyro_rad[2] - bias.gyro_z_bias;
msg.xmag = sensor.magnetometer_ga[0] - bias.mag_x_bias;
msg.ymag = sensor.magnetometer_ga[1] - bias.mag_y_bias;
msg.zmag = sensor.magnetometer_ga[2] - bias.mag_z_bias;
msg.abs_pressure = 0;
msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_x_bias;
msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_y_bias;
msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_z_bias;
msg.abs_pressure = air_data.baro_pressure_pa;
msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
msg.pressure_alt = sensor.baro_alt_meter;
msg.temperature = sensor.baro_temp_celcius;
msg.pressure_alt = air_data.baro_alt_meter;
msg.temperature = air_data.baro_temp_celcius;
msg.fields_updated = fields_updated;
mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
@@ -1025,8 +1038,7 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_sensor_sub;
uint64_t _sensor_time;
MavlinkOrbSubscription *_air_data_sub;
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
@@ -1044,7 +1056,7 @@ protected:
_act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_time(0),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
_air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data)))
{}
bool send(const hrt_abstime t)
@@ -1092,12 +1104,12 @@ protected:
msg.alt = -pos.z + pos.ref_alt;
} else {
sensor_combined_s sensor = {};
_sensor_sub->update(&sensor);
vehicle_air_data_s air_data = {};
_air_data_sub->update(&air_data);
/* fall back to baro altitude */
if (sensor.timestamp > 0) {
msg.alt = sensor.baro_alt_meter;
if (air_data.timestamp > 0) {
msg.alt = air_data.baro_alt_meter;
}
}
@@ -1714,7 +1726,7 @@ private:
uint64_t _lpos_time;
MavlinkOrbSubscription *_home_sub;
MavlinkOrbSubscription *_sensor_sub;
MavlinkOrbSubscription *_air_data_sub;
/* do not allow top copying this class */
MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &);
@@ -1727,7 +1739,7 @@ protected:
_lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
_lpos_time(0),
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
_air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data)))
{}
bool send(const hrt_abstime t)
@@ -1746,11 +1758,11 @@ protected:
} else {
// fall back to baro altitude
sensor_combined_s sensor = {};
_sensor_sub->update(&sensor);
vehicle_air_data_s air_data = {};
_air_data_sub->update(&air_data);
if (sensor.timestamp > 0) {
msg.alt = sensor.baro_alt_meter * 1000.0f;
if (air_data.timestamp > 0) {
msg.alt = air_data.baro_alt_meter * 1000.0f;
}
}
@@ -3710,7 +3722,7 @@ public:
private:
MavlinkOrbSubscription *_local_pos_sub;
MavlinkOrbSubscription *_home_sub;
MavlinkOrbSubscription *_sensor_sub;
MavlinkOrbSubscription *_air_data_sub;
uint64_t _local_pos_time{0};
@@ -3722,7 +3734,7 @@ protected:
explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink),
_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined)))
_air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data)))
{}
bool send(const hrt_abstime t)
@@ -3737,14 +3749,14 @@ protected:
msg.bottom_clearance = NAN;
// always update monotonic altitude
bool sensor_updated = false;
sensor_combined_s sensor = {};
_sensor_sub->update(&sensor);
bool air_data_updated = false;
vehicle_air_data_s air_data = {};
_air_data_sub->update(&air_data);
if (sensor.timestamp > 0) {
msg.altitude_monotonic = sensor.baro_alt_meter;
if (air_data.timestamp > 0) {
msg.altitude_monotonic = air_data.baro_alt_meter;
sensor_updated = true;
air_data_updated = true;
}
bool lpos_updated = false;
@@ -3786,7 +3798,7 @@ protected:
// avoid publishing only baro altitude_monotonic if possible
bool lpos_timeout = (hrt_elapsed_time(&_local_pos_time) > 10000);
if (lpos_updated || (sensor_updated && lpos_timeout)) {
if (lpos_updated || (air_data_updated && lpos_timeout)) {
msg.time_usec = hrt_absolute_time();
mavlink_msg_altitude_send_struct(_mavlink->get_channel(), &msg);