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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/navigator/loiter.cpp
View File

@ -100,11 +100,22 @@ Loiter::set_loiter_position()
_loiter_pos_set = true;
// set current mission item to loiter
set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
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
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item);

17
src/modules/navigator/mission.cpp
View File

@ -683,11 +683,22 @@ Mission::set_mission_items()
_mission_type = MISSION_TYPE_NONE;
/* set loiter mission item and ensure that there is a minimum clearance from home */
set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
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 */
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);

56
src/modules/navigator/mission_block.cpp
View File

@ -644,42 +644,40 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
}
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) {
/* landed, don't takeoff, but switch to IDLE mode */
item->nav_cmd = NAV_CMD_IDLE;
setLoiterItemCommonFields(item);
} else {
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
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) {
/* use current position setpoint */
item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
void
MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
{
setLoiterItemCommonFields(item);
} else {
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
item->loiter_radius = _navigator->get_loiter_radius();
}
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
item->altitude = _navigator->get_home_position()->alt + min_clearance;
}
}
void
MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
{
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
item->altitude_is_relative = false;
item->acceptance_radius = _navigator->get_acceptance_radius();
item->yaw = NAN;
item->time_inside = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
void

9
src/modules/navigator/mission_block.h
View File

@ -108,10 +108,11 @@ protected:
*/
bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
/**
* Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
*/
void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
void setLoiterItemCommonFields(struct mission_item_s *item);
/**
* Set a takeoff mission item

18
src/modules/navigator/takeoff.cpp
View File

@ -66,11 +66,25 @@ Takeoff::on_active()
_navigator->get_mission_result()->finished = true;
_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(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (_navigator->get_land_detected()->landed) {
_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_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}