dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

position_estimator_inav: reset reference altitude on arming.

This commit is contained in:
Anton Babushkin 2013-08-26 22:08:56 +02:00
parent e88d63ef27
commit b29d13347a
1 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
static bool verbose_mode = false;
static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s
static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float alt_avg = 0.0f;
bool landed = true;
hrt_abstime landed_time = 0;
bool flag_armed = false;
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
uint16_t flow_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
uint32_t updates_counter_len = 1000000;
hrt_abstime pub_last = hrt_absolute_time();
uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
// new ground level
baro_alt0 += sonar_corr;
warnx("new home: alt = %.3f", baro_alt0);
mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
t_prev = t;
/* reset ground level on arm */
if (armed.armed && !flag_armed) {
baro_alt0 -= z_est[0];
z_est[0] = 0.0f;
local_pos.ref_alt = baro_alt0;
local_pos.ref_timestamp = hrt_absolute_time();
mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
}
/* accel bias correction, now only for Z
* not strictly correct, but stable and works */
accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
flag_armed = armed.armed;
}
warnx("exiting.");