Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Navigator: land: fix when flying without global position estimate #23845

Merged
merged 3 commits into from
Oct 25, 2024
Merged

Navigator: land: fix when flying without global position estimate #23845

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a5f666b
Navigator: land: set lat/lon fields of triplet to NAN if global posit…
sfuhrer Oct 23, 2024
ea98e1d
navigator land: don't project VTOL braking waypoint without global po…
MaEtUgR Oct 24, 2024
1543bc0
navigator: refactor naming of break instead of brake functions
MaEtUgR Oct 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 7 additions & 6 deletions src/modules/navigator/land.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,9 @@ Land::on_activation()
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon);
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& _navigator->get_local_position()->xy_global) { // only execute if global position is valid
sfuhrer marked this conversation as resolved.
Show resolved Hide resolved
_navigator->preproject_stop_point(_mission_item.lat, _mission_item.lon);
}

mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
Expand All @@ -82,13 +83,13 @@ void
Land::on_active()
{
/* for VTOL update landing location during back transition */
if (_navigator->get_vstatus()->is_vtol &&
_navigator->get_vstatus()->in_transition_mode) {
if (_navigator->get_vstatus()->is_vtol
&& _navigator->get_vstatus()->in_transition_mode
&& _navigator->get_local_position()->xy_global) {
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

// create a wp in front of the VTOL while in back-transition, based on MPC settings that will apply in MC phase afterwards
_navigator->calculate_breaking_stop(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);

_navigator->preproject_stop_point(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
_navigator->set_position_setpoint_triplet_updated();
}

Expand Down
2 changes: 1 addition & 1 deletion src/modules/navigator/loiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ Loiter::set_loiter_position()
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);

} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
setLoiterItemFromCurrentPositionWithBraking(&_mission_item);

} else {
setLoiterItemFromCurrentPosition(&_mission_item);
Expand Down
15 changes: 11 additions & 4 deletions src/modules/navigator/mission_block.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -772,11 +772,11 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
}

void
MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item)
MissionBlock::setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item)
{
setLoiterItemCommonFields(item);

_navigator->calculate_breaking_stop(item->lat, item->lon);
_navigator->preproject_stop_point(item->lat, item->lon);

item->altitude = _navigator->get_global_position()->alt;
item->loiter_radius = _navigator->get_loiter_radius();
Expand Down Expand Up @@ -832,8 +832,15 @@ MissionBlock::set_land_item(struct mission_item_s *item)
item->nav_cmd = NAV_CMD_LAND;

// set land item to current position
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
if (_navigator->get_local_position()->xy_global) {
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;

} else {
item->lat = (double)NAN;
item->lon = (double)NAN;
}

item->yaw = NAN;

item->altitude = 0;
Expand Down
2 changes: 1 addition & 1 deletion src/modules/navigator/mission_block.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ class MissionBlock : public NavigatorMode
void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);

void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
void setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item);
void setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item);

void setLoiterItemCommonFields(struct mission_item_s *item);

Expand Down
2 changes: 1 addition & 1 deletion src/modules/navigator/navigator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -278,7 +278,7 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
void release_gimbal_control();
void set_gimbal_neutral();

void calculate_breaking_stop(double &lat, double &lon);
void preproject_stop_point(double &lat, double &lon);

void stop_capturing_images();
void disable_camera_trigger();
Expand Down
6 changes: 3 additions & 3 deletions src/modules/navigator/navigator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -356,7 +356,7 @@ void Navigator::run()
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {

calculate_breaking_stop(rep->current.lat, rep->current.lon);
preproject_stop_point(rep->current.lat, rep->current.lon);

} else {
// For fixedwings we can use the current vehicle's position to define the loiter point
Expand Down Expand Up @@ -467,7 +467,7 @@ void Navigator::run()
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {

calculate_breaking_stop(rep->current.lat, rep->current.lon);
preproject_stop_point(rep->current.lat, rep->current.lon);
}

if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
Expand Down Expand Up @@ -1588,7 +1588,7 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
return true;
}

void Navigator::calculate_breaking_stop(double &lat, double &lon)
void Navigator::preproject_stop_point(double &lat, double &lon)
{
// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
Expand Down
Loading