diff options
3 files changed, 63 insertions, 43 deletions
diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 292cf7f21..97d669612 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -281,7 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -300,8 +299,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); + struct vehicle_local_position_s pos; + memset(&pos, 0, sizeof(pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -311,10 +310,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t local_pos_est_pub = orb_advertise( - ORB_ID(vehicle_local_position), &local_pos_est); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); - struct position_estimator_inav_params pos_inav_params; + struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); @@ -324,21 +322,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ /* FIRST PARAMETER UPDATE */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - if (use_gps) { - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, - .events = POLLIN } }; + if (params.use_gps) { + struct pollfd fds_init[1] = { + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; while (gps.fix_type < 3) { if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } static int printcounter = 0; @@ -354,6 +352,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; + + pos.home_lat = lat_current * 1e7; + pos.home_lon = lon_current * 1e7; + pos.home_timestamp = hrt_absolute_time(); + /* initialize coordinates */ map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ @@ -376,16 +379,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN } }; + struct pollfd fds[5] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; - int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ printf("[position_estimator_inav] subscriptions poll error\n"); @@ -399,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -442,7 +447,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } } - if (use_gps) { + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ @@ -454,8 +459,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { + pos.valid = true; } } /* end of poll return value check */ @@ -466,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; acceleration_correct(accel_corr, sensor.accelerometer_raw, - pos_inav_params.acc_T, pos_inav_params.acc_offs); + params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -491,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + kalman_filter_inertial_update(z_est, z_meas, params.k, use_z); } if (verbose_mode) { @@ -513,21 +521,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0f; - local_pos_est.vx = 0.0f; - local_pos_est.y = 0.0f; - local_pos_est.vy = 0.0f; - local_pos_est.z = z_est[0]; - local_pos_est.vz = z_est[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) - && (isfinite(local_pos_est.y)) - && (isfinite(local_pos_est.vy)) - && (isfinite(local_pos_est.z)) - && (isfinite(local_pos_est.vz))) { + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; + pos.vy = 0.0f; + pos.z = z_est[0]; + pos.vz = z_est[1]; + pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID( - vehicle_local_position), local_pos_est_pub, - &local_pos_est); + vehicle_local_position), vehicle_local_position_pub, + &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index fb082fbcb..d3a165947 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -42,6 +42,8 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ +PARAM_DEFINE_INT32(INAV_USE_GPS, 0); + PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); @@ -63,8 +65,9 @@ PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); -int parameters_init(struct position_estimator_inav_param_handles *h) -{ +int parameters_init(struct position_estimator_inav_param_handles *h) { + h->use_gps = param_find("INAV_USE_GPS"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); h->k_alt_01 = param_find("INAV_K_ALT_01"); h->k_alt_10 = param_find("INAV_K_ALT_10"); @@ -88,8 +91,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) -{ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { + param_get(h->use_gps, &(p->use_gps)); + param_get(h->k_alt_00, &(p->k[0][0])); param_get(h->k_alt_01, &(p->k[0][1])); param_get(h->k_alt_10, &(p->k[1][0])); @@ -97,9 +101,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_0, &(p->acc_offs[0])); - param_get(h->acc_offs_1, &(p->acc_offs[1])); - param_get(h->acc_offs_2, &(p->acc_offs[2])); + /* read int32 and cast to int16 */ + int32_t t; + param_get(h->acc_offs_0, &t); + p->acc_offs[0] = t; + param_get(h->acc_offs_1, &t); + p->acc_offs[1] = t; + param_get(h->acc_offs_2, &t); + p->acc_offs[2] = t; param_get(h->acc_t_00, &(p->acc_T[0][0])); param_get(h->acc_t_01, &(p->acc_T[0][1])); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 694bf015b..51e57a914 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -41,12 +41,15 @@ #include <systemlib/param/param.h> struct position_estimator_inav_params { + int use_gps; float k[3][2]; int16_t acc_offs[3]; float acc_T[3][3]; }; struct position_estimator_inav_param_handles { + param_t use_gps; + param_t k_alt_00; param_t k_alt_01; param_t k_alt_10; |