mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 02:23:34 +08:00
Additional yaw compensation for flow module offset from center of rotation.
This commit is contained in:
ChristophTobler
@@ -31,7 +31,7 @@
|
||||
#
|
||||
############################################################################
|
||||
if(${OS} STREQUAL "nuttx")
|
||||
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3850)
|
||||
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
|
||||
endif()
|
||||
px4_add_module(
|
||||
MODULE modules__position_estimator_inav
|
||||
|
||||
@@ -322,16 +322,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered)
|
||||
hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered)
|
||||
|
||||
//----------test--------------------------
|
||||
float flow_test[] = { 0.0f, 0.0f };
|
||||
float pos_test[] = { 0.0f, 0.0f };
|
||||
float flow_test_average[] = { 0.0f, 0.0f };
|
||||
int n_flow = 0;
|
||||
float gyro_offset_filtered[] = { 0.0f, 0.0f };
|
||||
float flow_gyrospeed[] = { 0.0f, 0.0f };
|
||||
float flow_gyrospeed_filtered[] = { 0.0f, 0.0f };
|
||||
float att_gyrospeed_filtered[] = { 0.0f, 0.0f };
|
||||
//----------------------------------------
|
||||
float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };
|
||||
float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };
|
||||
float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
|
||||
float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
|
||||
float yaw_comp[] = { 0.0f, 0.0f };
|
||||
|
||||
bool gps_valid = false; // GPS is valid
|
||||
bool lidar_valid = false; // lidar is valid
|
||||
@@ -505,12 +501,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
|
||||
}
|
||||
}
|
||||
//------------test----------------------
|
||||
//dont convert into NED for tests
|
||||
//acc[0] = sensor.accelerometer_m_s2[0];
|
||||
//acc[1] = sensor.accelerometer_m_s2[1];
|
||||
//acc[2] = sensor.accelerometer_m_s2[2];
|
||||
//---------------------------------------
|
||||
|
||||
acc[2] += CONSTANTS_ONE_G;
|
||||
|
||||
@@ -580,7 +570,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
float flow_q = flow.quality / 255.0f;
|
||||
float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance;
|
||||
|
||||
if (dist_bottom > 0.21f && flow_q > params.flow_q_min /*&& (t < lidar_valid_time + lidar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f*/) {
|
||||
if (dist_bottom > 0.21f && flow_q > params.flow_q_min) {
|
||||
/* 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
|
||||
@@ -599,11 +589,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
//this flag is not working -->
|
||||
flow_accurate = true; //already checked if flow_q > 0.3
|
||||
|
||||
|
||||
/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
|
||||
flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f;
|
||||
flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f;
|
||||
flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f;
|
||||
|
||||
//moving average
|
||||
if (n_flow >= 100) {
|
||||
gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
|
||||
gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
|
||||
gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
|
||||
n_flow = 0;
|
||||
flow_gyrospeed_filtered[0] = 0.0f;
|
||||
flow_gyrospeed_filtered[1] = 0.0f;
|
||||
flow_gyrospeed_filtered[2] = 0.0f;
|
||||
att_gyrospeed_filtered[0] = 0.0f;
|
||||
att_gyrospeed_filtered[1] = 0.0f;
|
||||
att_gyrospeed_filtered[2] = 0.0f;
|
||||
} else {
|
||||
flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
|
||||
flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
|
||||
flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
|
||||
att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
|
||||
att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
|
||||
att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
|
||||
n_flow++;
|
||||
}
|
||||
|
||||
|
||||
/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
|
||||
yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
|
||||
yaw_comp[1] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
|
||||
|
||||
|
||||
/* convert raw flow to angular flow (rad/s) */
|
||||
float flow_ang[2];
|
||||
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
|
||||
flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
|
||||
flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
|
||||
flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
|
||||
flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
|
||||
/* flow measurements vector */
|
||||
float flow_m[3];
|
||||
flow_m[0] = -flow_ang[0] * flow_dist;
|
||||
@@ -619,43 +643,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
//-----------test-------------------
|
||||
//do not transform to NED for testing
|
||||
//flow_v[0] = flow_m[0];
|
||||
//flow_v[1] = flow_m[1];
|
||||
|
||||
flow_test[0] = flow_v[0];
|
||||
flow_test[1] = flow_v[1];
|
||||
|
||||
flow_test_average[0] = (flow.pixel_flow_x_integral + flow_test_average[0] * n_flow) / (n_flow + 1);
|
||||
flow_test_average[1] = (flow.pixel_flow_y_integral + flow_test_average[1] * n_flow) / (n_flow + 1);
|
||||
|
||||
//calculate offset of flow-gyro using already calibrated gyro from autopilot
|
||||
flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f;
|
||||
flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f;
|
||||
|
||||
|
||||
|
||||
//moving average
|
||||
if (n_flow >= 100) {
|
||||
gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
|
||||
gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
|
||||
n_flow = 0;
|
||||
flow_gyrospeed_filtered[0] = 0.0f;
|
||||
flow_gyrospeed_filtered[1] = 0.0f;
|
||||
att_gyrospeed_filtered[0] = 0.0f;
|
||||
att_gyrospeed_filtered[1] = 0.0f;
|
||||
} else {
|
||||
flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
|
||||
flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
|
||||
att_gyrospeed_filtered[0] = (att.rollspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
|
||||
att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
|
||||
n_flow++;
|
||||
//mavlink_log_info(mavlink_fd, "n_flow = %d\n", (int)n_flow);
|
||||
}
|
||||
|
||||
//----------------------------------
|
||||
|
||||
/* velocity correction */
|
||||
corr_flow[0] = flow_v[0] - x_est[1];
|
||||
corr_flow[1] = flow_v[1] - y_est[1];
|
||||
@@ -670,11 +657,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
w_flow *= 0.05f;
|
||||
}
|
||||
|
||||
//--------test-------------
|
||||
//mavlink_log_info(mavlink_fd, "flow_accurate = %d\t w_flow = %4.4f\n", (int)flow_accurate, (double)w_flow);
|
||||
//w_flow = 0.8;
|
||||
//-------------------------
|
||||
|
||||
/* under ideal conditions, on 1m distance assume EPH = 10cm */
|
||||
eph_flow = 0.1f / w_flow;
|
||||
|
||||
@@ -1282,27 +1264,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
buf_ptr = 0;
|
||||
}
|
||||
|
||||
//-------------test--------------
|
||||
pos_test[0] += flow_test[0] * dt;
|
||||
pos_test[1] += flow_test[1] * dt;
|
||||
//-------------------------------
|
||||
|
||||
/* publish local position */
|
||||
local_pos.xy_valid = can_estimate_xy;
|
||||
local_pos.v_xy_valid = can_estimate_xy;
|
||||
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
|
||||
local_pos.z_global = local_pos.z_valid && use_gps_z;
|
||||
local_pos.x = corr_baro; //test
|
||||
local_pos.x = x_est[0];
|
||||
local_pos.vx = surface_offset; // flow_test[0]; //test
|
||||
local_pos.vx = x_est[1];
|
||||
local_pos.y = acc_bias[2]; //test
|
||||
local_pos.y = y_est[0];
|
||||
local_pos.vy = flow_gyrospeed_filtered[0]; //test
|
||||
local_pos.vy = y_est[1];
|
||||
local_pos.z = lidar.current_distance; //flow_test[0]; //test
|
||||
local_pos.z = z_est[0];
|
||||
local_pos.vz = - z_est[0] - surface_offset; //flow_test[1]; //test
|
||||
local_pos.vz = z_est[1];
|
||||
local_pos.yaw = att.yaw;
|
||||
local_pos.dist_bottom_valid = dist_bottom_valid;
|
||||
@@ -1311,7 +1282,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
|
||||
if (local_pos.dist_bottom_valid) {
|
||||
local_pos.dist_bottom = -z_est[0] - surface_offset;
|
||||
local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
|
||||
local_pos.dist_bottom_rate = - z_est[1] - surface_offset_rate;
|
||||
}
|
||||
|
||||
local_pos.timestamp = t;
|
||||
|
||||
@@ -289,6 +289,30 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
|
||||
|
||||
/**
|
||||
* Flow module offset (center of rotation) in X direction
|
||||
*
|
||||
* Yaw X flow compensation
|
||||
*
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @unit m
|
||||
* @group Position Estimator INAV
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
|
||||
|
||||
/**
|
||||
* Flow module offset (center of rotation) in Y direction
|
||||
*
|
||||
* Yaw Y flow compensation
|
||||
*
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @unit m
|
||||
* @group Position Estimator INAV
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
|
||||
|
||||
/**
|
||||
* Disable vision input
|
||||
*
|
||||
@@ -338,6 +362,8 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
|
||||
h->land_thr = param_find("INAV_LAND_THR");
|
||||
h->no_vision = param_find("CBRK_NO_VISION");
|
||||
h->delay_gps = param_find("INAV_DELAY_GPS");
|
||||
h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
|
||||
h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -366,6 +392,8 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
|
||||
param_get(h->land_thr, &(p->land_thr));
|
||||
param_get(h->no_vision, &(p->no_vision));
|
||||
param_get(h->delay_gps, &(p->delay_gps));
|
||||
param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
|
||||
param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -65,6 +65,8 @@ struct position_estimator_inav_params {
|
||||
float land_thr;
|
||||
int32_t no_vision;
|
||||
float delay_gps;
|
||||
float flow_module_offset_x;
|
||||
float flow_module_offset_y;
|
||||
};
|
||||
|
||||
struct position_estimator_inav_param_handles {
|
||||
@@ -91,6 +93,8 @@ struct position_estimator_inav_param_handles {
|
||||
param_t land_thr;
|
||||
param_t no_vision;
|
||||
param_t delay_gps;
|
||||
param_t flow_module_offset_x;
|
||||
param_t flow_module_offset_y;
|
||||
};
|
||||
|
||||
#define CBRK_NO_VISION_KEY 328754
|
||||
|
||||
Reference in New Issue
Block a user