fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 02:29:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixing takeoff mission and swtiching to previous flight mode after land/takeoff

Browse Source
This commit is contained in:
Andreas Antener 2015-12-18 11:24:21 +01:00 committed by Lorenz Meier
parent 64349c7a09
commit f17c5d8d55
3 changed files with 20 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/commander/commander.cpp
View File

@ -2352,20 +2352,16 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* reset main state after takeoff and land */
if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) {
/* reset main state after takeoff or land has been completed */
/* only switch back to at least altitude controlled modes */
if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL ||
status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) {
if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX
&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) {
main_state_transition(&status, status.main_state_prev);
}
if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& mission_result.finished) ||
(status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
&& status.condition_landed)) {
} else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
&& status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX
&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) {
main_state_transition(&status, status.main_state_prev);
}
}

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

@ -119,8 +119,16 @@ MissionBlock::is_mission_item_reached()
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
/* _mission_item.acceptance_radius is not always set */
float mission_acceptance_radius = _mission_item.acceptance_radius;
/* if set to zero use the default instead */
if (mission_acceptance_radius < NAV_EPSILON_POSITION) {
mission_acceptance_radius = _navigator->get_acceptance_radius();
}
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
altitude_amsl - mission_acceptance_radius) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
@ -301,7 +309,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
{
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
item->nav_cmd = NAV_CMD_TAKEOFF;
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;

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

@ -78,6 +78,7 @@ Takeoff::on_activation()
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@ -96,7 +97,7 @@ Takeoff::on_activation()
void
Takeoff::on_active()
{
if (is_mission_item_reached()) {
if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
}