dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: taking off now on takeoff command, but not yet holding altitude once completed.

This commit is contained in:
Lorenz Meier 2015-11-23 00:23:13 +01:00
parent 34f4c57456
commit 57b9ac05de
3 changed files with 33 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -289,3 +289,29 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
item->origin = ORIGIN_ONBOARD;
}
}
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
{
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
/* 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;
if (min_clearance > 0.0f) {
item->altitude += min_clearance;
}
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ?
(min_clearance / 2) : _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->pitch_min = min_pitch;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}

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

@ -96,6 +96,11 @@ protected:
*/
void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
/**
* Set a takeoff mission item
*/
void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f);
mission_item_s _mission_item;
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;

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

@ -74,12 +74,13 @@ void
Takeoff::on_activation()
{
/* set current mission item to Takeoff */
set_loiter_item(&_mission_item, _param_min_alt.get());
set_takeoff_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->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);