From 8ae50a4ba524f840dc7d96b79c5dbad1b7be15f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 1/3] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea6118022..e9c036e858 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -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 */ @@ -410,6 +412,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); @@ -797,20 +800,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; From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 2/3] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: [PATCH 3/3] commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f2..077184bfbd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);