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

Merge branch 'beta_mavlink2' into mpc_local_pos_mavlink

Browse Source
This commit is contained in:
Anton Babushkin
2014-03-19 20:01:01 +04:00
parent 068b7526b7 295f87f22c
commit 9b71e660ad
49 changed files with 5122 additions and 4691 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+8 -8
View File
@@ -207,8 +207,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool landed = true;
hrt_abstime landed_time = 0;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
hrt_abstime accel_timestamp = 0;
hrt_abstime baro_timestamp = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
@@ -319,8 +319,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_counter != baro_counter) {
baro_counter = sensor.baro_counter;
if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp;
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
@@ -393,7 +393,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter != accel_counter) {
if (sensor.accelerometer_timestamp != accel_timestamp) {
if (att.R_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
@@ -417,13 +417,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(corr_acc, 0, sizeof(corr_acc));
}
accel_counter = sensor.accelerometer_counter;
accel_timestamp = sensor.accelerometer_timestamp;
accel_updates++;
}
if (sensor.baro_counter != baro_counter) {
if (sensor.baro_timestamp != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_timestamp = sensor.baro_timestamp;
baro_updates++;
}
}