Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

navigator: cleanup of set_loiter_item

Browse Source 
Unwraps the set_loiter_item() to solve the issue where the altitdue setpoint
in a MC takeoff wasn't correctly used.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-03-09 22:10:05 +03:00
committed by Roman Bapst
parent cb78ba34d7
commit bf6a47ba6a
5 changed files with 76 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/loiter.cpp
+14 -3
View File
@@ -100,11 +100,22 @@ Loiter::set_loiter_position()
_loiter_pos_set = true; _loiter_pos_set = true;
// set current mission item to loiter position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
if (_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
}
}
// convert mission item to current setpoint // convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false; pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.valid = false; pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item); mission_apply_limitation(_mission_item);
src/modules/navigator/mission.cpp
+14 -3
View File
@@ -683,11 +683,22 @@ Mission::set_mission_items()
_mission_type = MISSION_TYPE_NONE; _mission_type = MISSION_TYPE_NONE;
/* set loiter mission item and ensure that there is a minimum clearance from home */ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
if (_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
}
}
/* update position setpoint triplet */ /* update position setpoint triplet */
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false; pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item); mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
src/modules/navigator/mission_block.cpp
+27 -29
View File
@@ -644,42 +644,40 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
} }
void void
MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item)
{ {
if (_navigator->get_land_detected()->landed) { setLoiterItemCommonFields(item);
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;
} else { const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
item->loiter_radius = pos_sp_triplet->current.loiter_radius;
}
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) { void
/* use current position setpoint */ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
item->lat = pos_sp_triplet->current.lat; {
item->lon = pos_sp_triplet->current.lon; setLoiterItemCommonFields(item);
item->altitude = pos_sp_triplet->current.alt;
} else { item->lat = _navigator->get_global_position()->lat;
/* use current position and use return altitude as clearance */ item->lon = _navigator->get_global_position()->lon;
item->lat = _navigator->get_global_position()->lat; item->altitude = _navigator->get_global_position()->alt;
item->lon = _navigator->get_global_position()->lon; item->loiter_radius = _navigator->get_loiter_radius();
item->altitude = _navigator->get_global_position()->alt; }
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { void
item->altitude = _navigator->get_home_position()->alt + min_clearance; MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
} {
} item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
item->altitude_is_relative = false; item->altitude_is_relative = false;
item->yaw = NAN; item->acceptance_radius = _navigator->get_acceptance_radius();
item->loiter_radius = _navigator->get_loiter_radius(); item->yaw = NAN;
item->acceptance_radius = _navigator->get_acceptance_radius(); item->time_inside = 0.0f;
item->time_inside = 0.0f; item->autocontinue = false;
item->autocontinue = false; item->origin = ORIGIN_ONBOARD;
item->origin = ORIGIN_ONBOARD;
}
} }
void void
src/modules/navigator/mission_block.h
+5 -4
View File
@@ -108,10 +108,11 @@ protected:
*/ */
bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp); bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
/** void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
* Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
*/ void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
void setLoiterItemCommonFields(struct mission_item_s *item);
/** /**
* Set a takeoff mission item * Set a takeoff mission item
src/modules/navigator/takeoff.cpp
+16 -2
View File
@@ -66,11 +66,25 @@ Takeoff::on_active()
_navigator->get_mission_result()->finished = true; _navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated(); _navigator->set_mission_result_updated();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// set loiter item so position controllers stop doing takeoff logic // set loiter item so position controllers stop doing takeoff logic
set_loiter_item(&_mission_item); if (_navigator->get_land_detected()->landed) {
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); _mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (pos_sp_triplet->current.valid) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
}
}
mission_apply_limitation(_mission_item); mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated(); _navigator->set_position_setpoint_triplet_updated();
} }
} }