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

FW move altitude first order hold (FOH) and loiter to position special cases from navigator to position controller

Browse Source 
Co-authored-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Daniel Agar
2020-10-06 16:57:12 -04:00
committed by GitHub
co-authored by RomanBapst
parent 9838e340e0
commit c01fabaf11
7 changed files with 125 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
-90
View File
@@ -222,10 +222,6 @@ Mission::on_active()
set_mission_items();
}
} else if (_mission_type != MISSION_TYPE_NONE && _param_mis_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -584,9 +580,6 @@ Mission::advance_mission()
void
Mission::set_mission_items()
{
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
_min_current_sp_distance_xy = FLT_MAX;
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
@@ -1152,14 +1145,6 @@ Mission::set_mission_items()
}
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
@@ -1354,81 +1339,6 @@ Mission::heading_sp_update()
}
}
void
Mission::altitude_sp_foh_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid
* or if the previous altitude isn't from a position or loiter setpoint or
* if rotary wing since that is handled in the mc_pos_control
*/
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt)
|| !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
return;
}
// Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore
float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f);
}
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - acc_rad < FLT_EPSILON) {
return;
}
/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic
*
* NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
* */
if (_mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
|| !_navigator->is_planned_mission()) {
return;
}
/* Calculate distance to current waypoint */
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
_distance_current_previous);
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < acc_rad) {
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
float grad = -delta_alt / (_distance_current_previous - acc_rad);
float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
}
// we set altitude directly so we can run this in parallel to the heading update
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::cruising_speed_sp_update()
{