diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1ddedddff4..6ecf73a606 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -135,7 +135,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } - bool home_position_valid() { return _home_position_set; } + bool home_position_valid() { return (_home_pos.timestamp > 0); } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } @@ -205,8 +205,6 @@ private: geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; - bool _home_position_set; - bool _mission_item_valid; /**< flags if the current mission item is valid */ int _mission_instance_count; /**< instance count for the current mission */ @@ -257,7 +255,7 @@ private: /** * Retrieve home position */ - void home_position_update(); + void home_position_update(bool force=false); /** * Retreive navigation capabilities diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 92f3e2fecb..9131fb16f0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,7 +126,6 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, - _home_position_set(false), _mission_item_valid(false), _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -206,17 +205,13 @@ Navigator::sensor_combined_update() } void -Navigator::home_position_update() +Navigator::home_position_update(bool force) { bool updated = false; orb_check(_home_pos_sub, &updated); - if (updated) { + if (updated || force) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - if (_home_pos.timestamp > 0) { - _home_position_set = true; - } } } @@ -298,7 +293,7 @@ Navigator::task_main() global_position_update(); gps_position_update(); sensor_combined_update(); - home_position_update(); + home_position_update(true); navigation_capabilities_update(); params_update(); @@ -408,7 +403,7 @@ Navigator::task_main() if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false;