fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 04:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: Initialize home position at boot time

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-17 09:32:35 +01:00
parent 907848452f
commit 81ff8d8060
2 changed files with 6 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator.h
+2 -4
View File
@@ -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
src/modules/navigator/navigator_main.cpp
+4 -9
View File
@@ -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;