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

adapted to new vehicle attitude message

Browse Source
This commit is contained in:
tumbili
2016-10-18 20:46:08 +02:00
committed by Lorenz Meier
parent 215bfaa377
commit 5e0e522903
16 changed files with 142 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+20 -14
View File
@@ -500,11 +500,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* sensor combined */
orb_check(sensor_combined_sub, &updated);
matrix::Quaternion<float> q(&att.q[0]);
matrix::Dcm<float> R(q);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
if (att.R_valid) {
if (att.q_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
@@ -515,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
acc[i] += R(i, j) * sensor.accelerometer_m_s2[j];
}
}
@@ -548,9 +551,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance
&& lidar.current_distance < lidar.max_distance
&& (PX4_R(att.R, 2, 2) > 0.7f)) {
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
&& (R(2, 2) > 0.7f)) {
if (!use_lidar_prev && use_lidar) {
lidar_first = true;
@@ -559,7 +561,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
use_lidar_prev = use_lidar;
lidar_time = t;
dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
dist_ground = lidar.current_distance * R(2, 2); //vertical distance
if (lidar_first) {
lidar_first = false;
@@ -602,7 +604,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = lidar.current_distance;
if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && R(2, 2) > 0.7f) {
/* distance to surface */
//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
float flow_dist = dist_bottom; //use this if using lidar
@@ -612,7 +614,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
body_v_est[i] = R( 0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -706,7 +708,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
flow_v[i] += R(i, j) * flow_m[j];
}
}
@@ -715,7 +717,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
w_flow = R(2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
@@ -946,6 +948,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
matrix::Quaternion<float> q(&att.q[0]);
matrix::Dcm<float> R(q);
/* check for timeout on FLOW topic */
if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
flow_valid = false;
@@ -1110,7 +1115,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
c += R(j, i) * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
@@ -1140,7 +1145,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
c += R(j, i) * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
@@ -1311,7 +1316,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
est_buf[buf_ptr][2][1] = z_est[1];
/* push current rotation matrix to buffer */
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
memcpy(R_buf[buf_ptr], &R._data[0][0], sizeof(R._data));
buf_ptr++;
@@ -1331,7 +1336,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.yaw = att.yaw;
matrix::Euler<float> euler(R);
local_pos.yaw = euler(0);
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;