diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-20 03:43:37 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-20 03:43:37 +0200 |
| commit | 200bd8e3dd2f67d45d9ef83d8b9f54e159b7eb57 (patch) | |
| tree | e0357fca35f4a0295e368a939b3b51f4ea670a92 /src/modules/ekf_att_pos_estimator | |
| parent | b37d0f8f2e3cb0962155113b07a01830c189d4ce (diff) | |
| parent | 7cad27a0243a806aa374ffda4ef9e99a854e1c16 (diff) | |
| download | px4-firmware-200bd8e3dd2f67d45d9ef83d8b9f54e159b7eb57.tar.gz px4-firmware-200bd8e3dd2f67d45d9ef83d8b9f54e159b7eb57.tar.bz2 px4-firmware-200bd8e3dd2f67d45d9ef83d8b9f54e159b7eb57.zip | |
Merge branch 'ekf_params' of github.com:PX4/Firmware into ekf_params
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 22 |
1 files changed, 16 insertions, 6 deletions
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9d00f1d17..0dafc4311 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/estimator_status.h> #include <uORB/topics/actuator_armed.h> +#include <uORB/topics/home_position.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <geo/geo.h> @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -477,6 +479,7 @@ FixedwingEstimator::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -893,20 +896,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; |