fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 03:41:36 +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-28 15:21:50 +01:00
committed by Lorenz Meier
parent 64349c7a09
commit f17c5d8d55
3 changed files with 20 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+8 -12
View File
@@ -2352,20 +2352,16 @@ int commander_thread_main(int argc, char *argv[])
} }
} }
/* reset main state after takeoff and land */ /* reset main state after takeoff or land has been completed */
if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF /* only switch back to at least altitude controlled modes */
&& mission_result.finished) { 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 if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) { && mission_result.finished) ||
main_state_transition(&status, status.main_state_prev); (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); main_state_transition(&status, status.main_state_prev);
} }
} }
src/modules/navigator/mission_block.cpp
+10 -2
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) { if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */ /* 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 > if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) { altitude_amsl - mission_acceptance_radius) {
_waypoint_position_reached = true; _waypoint_position_reached = true;
} }
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { } 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 void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) 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 */ /* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat; item->lat = _navigator->get_global_position()->lat;
src/modules/navigator/takeoff.cpp
+2 -1
View File
@@ -78,6 +78,7 @@ Takeoff::on_activation()
_navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->finished = false;
_navigator->set_mission_result_updated(); _navigator->set_mission_result_updated();
reset_mission_item_reached();
/* convert mission item to current setpoint */ /* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -96,7 +97,7 @@ Takeoff::on_activation()
void void
Takeoff::on_active() 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->get_mission_result()->finished = true;
_navigator->set_mission_result_updated(); _navigator->set_mission_result_updated();
} }