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: differentiate between takeoff and reposition commands, perform calculations for repositions only when armed.

Browse Source
This commit is contained in:
Lorenz Meier 2016-05-10 13:58:42 +02:00
parent befab7303f
commit 8960ab3402
6 changed files with 119 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,7 +58,8 @@
Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_min_alt(this, "MIS_LTRMIN_ALT", false)
_param_min_alt(this, "MIS_LTRMIN_ALT", false),
_loiter_pos_set(false)
{
// load initial params
updateParams();
@ -71,6 +72,7 @@ Loiter::~Loiter()
void
Loiter::on_inactive()
{
_loiter_pos_set = false;
}
void
@ -79,19 +81,7 @@ Loiter::on_activation()
if (_navigator->get_reposition_triplet()->current.valid) {
reposition();
} else {
// set current mission item to loiter
set_loiter_item(&_mission_item, _param_min_alt.get());
// 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_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
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();
set_loiter_position();
}
}
@ -101,11 +91,46 @@ Loiter::on_active()
if (_navigator->get_reposition_triplet()->current.valid) {
reposition();
}
// reset the loiter position if we get disarmed
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
_loiter_pos_set = false;
}
}
void
Loiter::set_loiter_position()
{
// not setting loiter position until armed
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED ||
_loiter_pos_set) {
return;
} else {
_loiter_pos_set = true;
}
// set current mission item to loiter
set_loiter_item(&_mission_item, _param_min_alt.get());
// 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_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
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();
}
void
Loiter::reposition()
{
// we can't reposition if we are not armed yet
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
return;
}
struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
@ -115,7 +140,10 @@ Loiter::reposition()
// 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;
memcpy(&pos_sp_triplet->previous, &rep->previous, sizeof(rep->previous));
pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
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->next.valid = false;

6
src/modules/navigator/loiter.h
View File

@ -67,7 +67,13 @@ private:
*/
void reposition();
/**
* Set the position to hold based on the current local position
*/
void set_loiter_position();
control::BlockParamFloat _param_min_alt;
bool _loiter_pos_set;
};
#endif

2
src/modules/navigator/navigator.h
View File

@ -143,6 +143,7 @@ public:
bool home_position_valid() { return (_home_pos.timestamp > 0); }
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 mission_result_s* get_mission_result() { return &_mission_result; }
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
@ -231,6 +232,7 @@ private:
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
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 */
mission_result_s _mission_result;
geofence_result_s _geofence_result;

26
src/modules/navigator/navigator_main.cpp
View File

@ -129,6 +129,7 @@ Navigator::Navigator() :
_nav_caps{},
_pos_sp_triplet{},
_reposition_triplet{},
_takeoff_triplet{},
_mission_result{},
_att_sp{},
_mission_item_valid(false),
@ -415,7 +416,7 @@ Navigator::task_main()
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = NAN;
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
@ -448,9 +449,28 @@ Navigator::task_main()
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
rep->current.yaw = cmd.param4;
rep->current.lat = cmd.param5 / (double)1e7;
rep->current.lon = cmd.param6 / (double)1e7;
rep->current.alt = cmd.param7;
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
warnx("navigator: got pause/continue command");
}
}

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

@ -57,7 +57,7 @@ Takeoff::Takeoff(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
{
/* load initial params */
// load initial params
updateParams();
}
@ -72,6 +72,32 @@ Takeoff::on_inactive()
void
Takeoff::on_activation()
{
set_takeoff_position();
}
void
Takeoff::on_active()
{
struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
if (rep->current.valid) {
// reset the position
set_takeoff_position();
} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
// 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();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}
void
Takeoff::set_takeoff_position()
{
// set current mission item to takeoff
set_takeoff_item(&_mission_item, _param_min_alt.get());
@ -89,9 +115,22 @@ Takeoff::on_activation()
pos_sp_triplet->next.valid = false;
// check if a specific target altitude has been set
struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
pos_sp_triplet->current.alt = rep->current.alt;
struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
if (rep->current.valid) {
if (PX4_ISFINITE(rep->current.alt)) {
pos_sp_triplet->current.alt = rep->current.alt;
}
// Go on and check which changes had been requested
if (PX4_ISFINITE(rep->current.yaw)) {
pos_sp_triplet->current.yaw = rep->current.yaw;
}
if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) {
pos_sp_triplet->current.lat = rep->current.lat;
pos_sp_triplet->current.lon = rep->current.lon;
}
// mark this as done
memset(rep, 0, sizeof(*rep));
}
@ -100,18 +139,3 @@ Takeoff::on_activation()
_navigator->set_position_setpoint_triplet_updated();
}
void
Takeoff::on_active()
{
if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
/* 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();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}

2
src/modules/navigator/takeoff.h
View File

@ -62,6 +62,8 @@ public:
private:
control::BlockParamFloat _param_min_alt;
void set_takeoff_position();
};
#endif