fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 07:03:34 +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 = {};
src/modules/mavlink/mavlink_receiver.cpp
-73
View File
@@ -137,7 +137,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_follow_target_pub(nullptr),
_transponder_report_pub(nullptr),
_collision_report_pub(nullptr),
_control_state_pub(nullptr),
_debug_key_value_pub(nullptr),
_debug_value_pub(nullptr),
_debug_vect_pub(nullptr),
@@ -1188,9 +1187,6 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
vision_position.timestamp = sync_stamp(pos.time_usec);
// Use the estimator type to identify the external estimate
vision_position.estimator_type = pos.estimator_type;
vision_position.xy_valid = true;
vision_position.z_valid = true;
vision_position.v_xy_valid = true;
@@ -1204,10 +1200,6 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
vision_position.vy = pos.vy;
vision_position.vz = pos.vz;
vision_position.ax = pos.ax;
vision_position.ay = pos.ay;
vision_position.az = pos.az;
// Low risk change for now. TODO : full covariance matrix
vision_position.eph = sqrtf(fmaxf(pos.covariance[0], pos.covariance[9]));
vision_position.epv = sqrtf(pos.covariance[17]);
@@ -1232,9 +1224,6 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
struct vehicle_local_position_s vision_position = {};
// Use the estimator type to identify the simple vision estimate
vision_position.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
vision_position.timestamp = sync_stamp(pos.usec);
vision_position.x = pos.x;
vision_position.y = pos.y;
@@ -2220,7 +2209,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* global position */
{
struct vehicle_global_position_s hil_global_pos = {};
matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
hil_global_pos.timestamp = timestamp;
hil_global_pos.lat = hil_state.lat / ((double)1e7);
@@ -2229,7 +2217,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = euler.psi();
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
@@ -2338,66 +2325,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
/* control state */
control_state_s ctrl_state = {};
matrix::Quatf q(hil_state.attitude_quaternion);
matrix::Dcmf R_to_body(q.inversed());
//Time
ctrl_state.timestamp = hrt_absolute_time();
//Roll Rates:
//ctrl_state: body angular rate (rad/s, x forward/y right/z down)
//hil_state : body frame angular speed (rad/s)
ctrl_state.roll_rate = hil_state.rollspeed;
ctrl_state.pitch_rate = hil_state.pitchspeed;
ctrl_state.yaw_rate = hil_state.yawspeed;
// Local Position NED:
//ctrl_state: position in local earth frame
//hil_state : Latitude/Longitude expressed as * 1E7
float x = 0.0f;
float y = 0.0f;
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
ctrl_state.x_pos = x;
ctrl_state.y_pos = y;
ctrl_state.z_pos = hil_state.alt / 1000.0f;
// Attitude quaternion
q.copyTo(ctrl_state.q);
// Velocity
//ctrl_state: velocity in body frame (x forward/y right/z down)
//hil_state : Ground Speed in NED expressed as m/s * 100
matrix::Vector3f v_n(hil_state.vx, hil_state.vy, hil_state.vz);
matrix::Vector3f v_b = R_to_body * v_n;
ctrl_state.x_vel = v_b(0) / 100.0f;
ctrl_state.y_vel = v_b(1) / 100.0f;
ctrl_state.z_vel = v_b(2) / 100.0f;
// Acceleration
//ctrl_state: acceleration in body frame
//hil_state : acceleration in body frame
ctrl_state.x_acc = hil_state.xacc;
ctrl_state.y_acc = hil_state.yacc;
ctrl_state.z_acc = hil_state.zacc;
static float _acc_hor_filt = 0;
_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(ctrl_state.x_acc * ctrl_state.x_acc + ctrl_state.y_acc *
ctrl_state.y_acc);
ctrl_state.horz_acc_mag = _acc_hor_filt;
ctrl_state.airspeed_valid = false;
// publish control state data
if (_control_state_pub == nullptr) {
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
}
}
void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
src/modules/mavlink/mavlink_receiver.h
-3
View File
@@ -77,10 +77,8 @@
#include <uORB/topics/follow_target.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/collision_report.h>
#include "mavlink_mission.h"
#include "mavlink_parameters.h"
#include "mavlink_ftp.h"
@@ -243,7 +241,6 @@ private:
orb_advert_t _follow_target_pub;
orb_advert_t _transponder_report_pub;
orb_advert_t _collision_report_pub;
orb_advert_t _control_state_pub;
orb_advert_t _debug_key_value_pub;
orb_advert_t _debug_value_pub;
orb_advert_t _debug_vect_pub;