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

delete control_state and cleanup vehicle_attitude (#7882)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-21 16:24:53 -04:00
committed by GitHub
parent 5bea264a5f
commit b4755297ec
42 changed files with 776 additions and 1061 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+18 -10
View File
@@ -77,6 +77,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
@@ -632,6 +633,9 @@ private:
MavlinkOrbSubscription *_sensor_sub;
uint64_t _sensor_time;
MavlinkOrbSubscription *_bias_sub;
uint64_t _bias_time;
MavlinkOrbSubscription *_differential_pressure_sub;
uint64_t _differential_pressure_time;
@@ -648,6 +652,8 @@ protected:
explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
_sensor_time(0),
_bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))),
_bias_time(0),
_differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))),
_differential_pressure_time(0),
_accel_timestamp(0),
@@ -659,6 +665,7 @@ protected:
bool send(const hrt_abstime t)
{
struct sensor_combined_s sensor = {};
struct sensor_bias_s bias = {};
struct differential_pressure_s differential_pressure = {};
if (_sensor_sub->update(&_sensor_time, &sensor)) {
@@ -688,20 +695,21 @@ protected:
_baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
}
_bias_sub->update(&_bias_time, &bias);
_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
mavlink_highres_imu_t msg = {};
msg.time_usec = sensor.timestamp;
msg.xacc = sensor.accelerometer_m_s2[0];
msg.yacc = sensor.accelerometer_m_s2[1];
msg.zacc = sensor.accelerometer_m_s2[2];
msg.xgyro = sensor.gyro_rad[0];
msg.ygyro = sensor.gyro_rad[1];
msg.zgyro = sensor.gyro_rad[2];
msg.xmag = sensor.magnetometer_ga[0];
msg.ymag = sensor.magnetometer_ga[1];
msg.zmag = sensor.magnetometer_ga[2];
msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_x_bias;
msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_y_bias;
msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_z_bias;
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.diff_pressure = differential_pressure.differential_pressure_raw_pa;
msg.pressure_alt = sensor.baro_alt_meter;
@@ -3878,7 +3886,7 @@ protected:
msg.wind_y = wind_estimate.windspeed_east;
msg.wind_z = 0.0f;
msg.var_horiz = wind_estimate.covariance_north + wind_estimate.covariance_east;
msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east;
msg.var_vert = 0.0f;
struct vehicle_global_position_s global_pos = {};