mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 13:53:34 +08:00
navigator begin fixing code style
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
9e589cef48
commit
6f10f8de9a
+134
-123
@@ -94,8 +94,9 @@ RTL::on_activation()
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
|
||||
/* check if we are pretty close to home already */
|
||||
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
|
||||
_navigator->get_home_position()->lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
|
||||
/* decide where to enter the RTL procedure when we switch into it */
|
||||
if (_rtl_state == RTL_STATE_NONE) {
|
||||
@@ -104,12 +105,15 @@ RTL::on_activation()
|
||||
_rtl_state = RTL_STATE_LANDED;
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
|
||||
|
||||
/* if lower than return altitude, climb up first */
|
||||
} else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
|
||||
/* if lower than return altitude, climb up first */
|
||||
|
||||
} else if (home_dist > _param_rtl_min_dist.get()
|
||||
&& _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
|
||||
+ _param_return_alt.get()) {
|
||||
_rtl_state = RTL_STATE_CLIMB;
|
||||
|
||||
/* otherwise go straight to return */
|
||||
/* otherwise go straight to return */
|
||||
|
||||
} else {
|
||||
/* set altitude setpoint to current altitude */
|
||||
_rtl_state = RTL_STATE_RETURN;
|
||||
@@ -147,153 +151,157 @@ RTL::set_rtl_item()
|
||||
|
||||
switch (_rtl_state) {
|
||||
case RTL_STATE_CLIMB: {
|
||||
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
|
||||
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
|
||||
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = climb_alt;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = climb_alt;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
|
||||
(int)(climb_alt),
|
||||
(int)(climb_alt - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
|
||||
(int)(climb_alt),
|
||||
(int)(climb_alt - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_RETURN: {
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
// don't change altitude
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
// don't change altitude
|
||||
|
||||
// use home yaw if close to home
|
||||
/* check if we are pretty close to home already */
|
||||
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
// use home yaw if close to home
|
||||
/* check if we are pretty close to home already */
|
||||
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
|
||||
_navigator->get_home_position()->lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
|
||||
if (home_dist < _param_rtl_min_dist.get()) {
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
|
||||
} else {
|
||||
if (pos_sp_triplet->previous.valid) {
|
||||
/* if previous setpoint is valid then use it to calculate heading to home */
|
||||
_mission_item.yaw = get_bearing_to_next_waypoint(
|
||||
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
if (home_dist < _param_rtl_min_dist.get()) {
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
|
||||
} else {
|
||||
/* else use current position */
|
||||
_mission_item.yaw = get_bearing_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
if (pos_sp_triplet->previous.valid) {
|
||||
/* if previous setpoint is valid then use it to calculate heading to home */
|
||||
_mission_item.yaw = get_bearing_to_next_waypoint(
|
||||
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
|
||||
} else {
|
||||
/* else use current position */
|
||||
_mission_item.yaw = get_bearing_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
}
|
||||
}
|
||||
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
|
||||
(int)(_mission_item.altitude),
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
|
||||
_rtl_start_lock = true;
|
||||
break;
|
||||
}
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
|
||||
(int)(_mission_item.altitude),
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
|
||||
_rtl_start_lock = true;
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_TRANSITION_TO_MC: {
|
||||
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
|
||||
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
||||
break;
|
||||
}
|
||||
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
|
||||
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_DESCEND: {
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
|
||||
|
||||
// check if we are already lower - then we will just stay there
|
||||
if (_mission_item.altitude > _navigator->get_global_position()->alt) {
|
||||
_mission_item.altitude = _navigator->get_global_position()->alt;
|
||||
// check if we are already lower - then we will just stay there
|
||||
if (_mission_item.altitude > _navigator->get_global_position()->alt) {
|
||||
_mission_item.altitude = _navigator->get_global_position()->alt;
|
||||
}
|
||||
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
|
||||
// except for vtol which might be still off here and should point towards this location
|
||||
float d_current = get_distance_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
|
||||
if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
|
||||
_mission_item.yaw = get_bearing_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
}
|
||||
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.autocontinue = false;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
/* disable previous setpoint to prevent drift */
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
|
||||
(int)(_mission_item.altitude),
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
|
||||
// except for vtol which might be still off here and should point towards this location
|
||||
float d_current = get_distance_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
|
||||
if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
|
||||
_mission_item.yaw = get_bearing_to_next_waypoint(
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
_mission_item.lat, _mission_item.lon);
|
||||
}
|
||||
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.autocontinue = false;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
/* disable previous setpoint to prevent drift */
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
|
||||
(int)(_mission_item.altitude),
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_LOITER: {
|
||||
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
|
||||
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
|
||||
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
// don't change altitude
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
|
||||
_mission_item.autocontinue = autoland;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
// don't change altitude
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
|
||||
_mission_item.autocontinue = autoland;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
|
||||
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)Navigator::get_time_inside(_mission_item));
|
||||
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
|
||||
(double)Navigator::get_time_inside(_mission_item));
|
||||
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_LAND: {
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
set_land_item(&_mission_item, false);
|
||||
_mission_item.yaw = _navigator->get_home_position()->yaw;
|
||||
set_land_item(&_mission_item, false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
|
||||
break;
|
||||
}
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_LANDED: {
|
||||
set_idle_item(&_mission_item);
|
||||
set_idle_item(&_mission_item);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
|
||||
break;
|
||||
}
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
@@ -327,6 +335,7 @@ RTL::advance_rtl()
|
||||
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
|
||||
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case RTL_STATE_TRANSITION_TO_MC:
|
||||
@@ -334,6 +343,7 @@ RTL::advance_rtl()
|
||||
break;
|
||||
|
||||
case RTL_STATE_DESCEND:
|
||||
|
||||
/* only go to land if autoland is enabled */
|
||||
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
|
||||
_rtl_state = RTL_STATE_LOITER;
|
||||
@@ -341,6 +351,7 @@ RTL::advance_rtl()
|
||||
} else {
|
||||
_rtl_state = RTL_STATE_LAND;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case RTL_STATE_LOITER:
|
||||
|
||||
Reference in New Issue
Block a user