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

commander: centralise home position publication

Browse Source
This commit is contained in:
Paul Riseborough 2017-12-06 09:54:06 +11:00 committed by Lorenz Meier
parent 934a7af579
commit 41f3e1f9b4
1 changed files with 41 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/modules/commander/commander.cpp
View File

@ -1237,49 +1237,54 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude)
{
//Need global and local position fix to be able to set home
if (!status_flags.condition_global_position_valid || !status_flags.condition_local_position_valid) {
// Need global and local position fix to be able to set home
// Ensure that the GPS accuracy is good enough
if (status_flags.condition_global_position_valid
&& status_flags.condition_local_position_valid
&& (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold)) {
//Set home position
home.timestamp = hrt_absolute_time();
home.lat = globalPosition.lat;
home.lon = globalPosition.lon;
home.valid_hpos = true;
home.alt = globalPosition.alt;
home.valid_alt = true;
home.x = localPosition.x;
home.y = localPosition.y;
home.z = localPosition.z;
matrix::Eulerf euler = matrix::Quatf(attitude.q);
home.yaw = euler.psi();
PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
//Play tune first time we initialize HOME
if (!status_flags.condition_home_position_valid) {
tune_home_set(true);
}
/* mark home position as set */
status_flags.condition_home_position_valid = true;
} else if (!home.valid_alt && localPosition.z_global) {
// Handle case where we have started global height estimation after takeoff and can only set the home altitude
home.timestamp = hrt_absolute_time();
home.alt = localPosition.ref_alt;
home.valid_alt = true;
PX4_INFO("home alt: %.2f", (double)home.alt);
} else {
return;
}
//Ensure that the GPS accuracy is good enough for intializing home
if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
return;
}
//Set home position
home.timestamp = hrt_absolute_time();
home.lat = globalPosition.lat;
home.lon = globalPosition.lon;
home.valid_hpos = true;
home.alt = globalPosition.alt;
home.valid_alt = true;
home.x = localPosition.x;
home.y = localPosition.y;
home.z = localPosition.z;
matrix::Eulerf euler = matrix::Quatf(attitude.q);
home.yaw = euler.psi();
PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (homePub != nullptr) {
orb_publish(ORB_ID(home_position), homePub, &home);
} else {
homePub = orb_advertise(ORB_ID(home_position), &home);
}
//Play tune first time we initialize HOME
if (!status_flags.condition_home_position_valid) {
tune_home_set(true);
}
/* mark home position as set */
status_flags.condition_home_position_valid = true;
}
int commander_thread_main(int argc, char *argv[])
@ -3120,20 +3125,9 @@ int commander_thread_main(int argc, char *argv[])
/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
* This allows home atitude to be used in the calculation of height above takeoff location when GPS
* use has commenced after takeoff. */
if (!_home.valid_alt && local_position.z_global) {
_home.timestamp = hrt_absolute_time();
_home.alt = local_position.ref_alt;
_home.valid_alt = true;
else if (!_home.valid_alt && local_position.z_global) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
/* publish new home altitude */
if (home_pub != nullptr) {
orb_publish(ORB_ID(home_position), home_pub, &_home);
} else {
home_pub = orb_advertise(ORB_ID(home_position), &_home);
}
PX4_INFO("home alt: %.2f", (double)_home.alt);
}
// check for arming state change