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

[WIP] navigator initialization and mode change improvements #10611

Closed
wants to merge 1 commit into from
Closed

[WIP] navigator initialization and mode change improvements #10611

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 6 additions & 3 deletions src/modules/navigator/land.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ Land::on_activation()
reset_mission_item_reached();

/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_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);
Expand All @@ -73,19 +73,22 @@ Land::on_active()
/* for VTOL update landing location during back transition */
if (_navigator->get_vstatus()->is_vtol &&
_navigator->get_vstatus()->in_transition_mode) {
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;

_navigator->set_position_setpoint_triplet_updated();
}


if (_navigator->get_land_detected()->landed) {

_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
set_idle_item(&_mission_item);

struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
Expand Down
32 changes: 18 additions & 14 deletions src/modules/navigator/loiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ Loiter::on_inactive()
void
Loiter::on_activation()
{
if (_navigator->get_reposition_triplet()->current.valid) {
if (_reposition.valid) {
reposition();

} else {
Expand All @@ -68,7 +68,7 @@ Loiter::on_activation()
void
Loiter::on_active()
{
if (_navigator->get_reposition_triplet()->current.valid) {
if (_reposition.valid) {
reposition();
}

Expand Down Expand Up @@ -98,8 +98,6 @@ Loiter::set_loiter_position()
return;
}

_loiter_pos_set = true;

// set current mission item to loiter
set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());

Expand All @@ -113,6 +111,8 @@ Loiter::set_loiter_position()

_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();

_loiter_pos_set = true;
}

void
Expand All @@ -123,25 +123,29 @@ Loiter::reposition()
return;
}

struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();

if (rep->current.valid) {
if (_reposition.valid) {
// set loiter position based on reposition command

// 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.yaw = _navigator->get_global_position()->yaw;
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

// previous: current position
pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
pos_sp_triplet->previous.valid = true;

// current: use reposition
pos_sp_triplet->current = _reposition;
pos_sp_triplet->current.velocity_valid = false;

// next: invalid
pos_sp_triplet->next.valid = false;

_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();

// mark this as done
memset(rep, 0, sizeof(*rep));
// clear the reposition to mark as done
_reposition = position_setpoint_s{};
}
}
11 changes: 3 additions & 8 deletions src/modules/navigator/loiter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,14 +55,7 @@ class Loiter : public MissionBlock, public ModuleParams
void on_activation() override;
void on_active() override;

// TODO: share this with mission
enum mission_yaw_mode {
MISSION_YAWMODE_NONE = 0,
MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
MISSION_YAWMODE_FRONT_TO_HOME = 2,
MISSION_YAWMODE_BACK_TO_HOME = 3,
MISSION_YAWMODE_MAX = 4
};
position_setpoint_s &get_reposition() { return _reposition; }

private:
/**
Expand All @@ -76,5 +69,7 @@ class Loiter : public MissionBlock, public ModuleParams
*/
void set_loiter_position();

position_setpoint_s _reposition{}; /**< position setpoint for non-mission direct position command */

bool _loiter_pos_set{false};
};
8 changes: 0 additions & 8 deletions src/modules/navigator/mission.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,14 +80,6 @@ class Mission : public MissionBlock, public ModuleParams
MISSION_ALTMODE_FOH = 1
};

enum mission_yaw_mode {
MISSION_YAWMODE_NONE = 0,
MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
MISSION_YAWMODE_FRONT_TO_HOME = 2,
MISSION_YAWMODE_BACK_TO_HOME = 3,
MISSION_YAWMODE_MAX = 4
};

bool set_current_offboard_mission_index(uint16_t index);

bool land_start();
Expand Down
22 changes: 17 additions & 5 deletions src/modules/navigator/mission_block.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,14 @@ using matrix::wrap_pi;
MissionBlock::MissionBlock(Navigator *navigator) :
NavigatorMode(navigator)
{
_mission_item.lat = (double)NAN;
_mission_item.lon = (double)NAN;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
}

bool
Expand Down Expand Up @@ -469,10 +477,14 @@ MissionBlock::issue_command(const mission_item_s &item)
}

float
MissionBlock::get_time_inside(const struct mission_item_s &item)
MissionBlock::get_time_inside(const mission_item_s &item) const
{
if (item.nav_cmd != NAV_CMD_TAKEOFF) {
return item.time_inside;
if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) ||
item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item.nav_cmd == NAV_CMD_DELAY) {

// TODO: set appropriate upper limit
return math::constrain(item.time_inside, 0.0f, 3600.0f);
}

return 0.0f;
Expand Down Expand Up @@ -502,7 +514,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi

sp->lat = item.lat;
sp->lon = item.lon;
sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
sp->alt = get_absolute_altitude_for_item(item);
sp->yaw = item.yaw;
sp->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
Expand Down Expand Up @@ -728,7 +740,7 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
}

float
MissionBlock::get_absolute_altitude_for_item(struct mission_item_s &mission_item) const
MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
{
if (mission_item.altitude_is_relative) {
return mission_item.altitude + _navigator->get_home_position()->alt;
Expand Down
6 changes: 3 additions & 3 deletions src/modules/navigator/mission_block.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,13 +115,13 @@ class MissionBlock : public NavigatorMode
/**
* General function used to adjust the mission item based on vehicle specific limitations
*/
void mission_apply_limitation(mission_item_s &item);
void mission_apply_limitation(mission_item_s &item);

void issue_command(const mission_item_s &item);

float get_time_inside(const struct mission_item_s &item);
float get_time_inside(const mission_item_s &item) const ;

float get_absolute_altitude_for_item(struct mission_item_s &mission_item) const;
float get_absolute_altitude_for_item(const mission_item_s &mission_item) const;

mission_item_s _mission_item{};

Expand Down
8 changes: 2 additions & 6 deletions src/modules/navigator/navigator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,8 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
{
public:
Navigator();
virtual ~Navigator() = default;
~Navigator() override;

Navigator(const Navigator &) = delete;
Navigator operator=(const Navigator &) = delete;

Expand Down Expand Up @@ -149,8 +150,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
struct home_position_s *get_home_position() { return &_home_pos; }
struct mission_result_s *get_mission_result() { return &_mission_result; }
struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
Expand Down Expand Up @@ -324,8 +323,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
// Publications
geofence_result_s _geofence_result{};
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */
position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */
position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */
vehicle_roi_s _vroi{}; /**< vehicle ROI */

perf_counter_t _loop_perf; /**< loop performance counter */
Expand All @@ -335,7 +332,6 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams

bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */
bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */
bool _mission_result_updated{false}; /**< flags if mission result has seen an update */

NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
Expand Down
Loading