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

commander : add support for precision landing measurements

Browse Source
This commit is contained in:
Nicolas de Palezieux 2018-01-14 23:23:14 +05:30 committed by Lorenz Meier
parent d9b9b4407a
commit f2dbb0ad3b
2 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -581,6 +581,8 @@ int commander_main(int argc, char *argv[])
new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
} else if (!strcmp(argv[2], "auto:land")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
} else if (!strcmp(argv[2], "auto:precland")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
} else {
warnx("argument %s unsupported.", argv[2]);
}
@ -1109,6 +1111,18 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, main_state_prev, &status_flags, &internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(&mavlink_log_pub, "Precision landing denied, land manually");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
@ -3902,6 +3916,7 @@ set_control_mode()
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:

25
src/modules/commander/state_machine_helper.cpp
View File

@ -481,6 +481,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
/* need local and global position, and precision land only implemented for multicopters */
if (status_flags->condition_local_position_valid && status_flags->condition_global_position_valid && status->is_rotary_wing) {
ret = TRANSITION_CHANGED;
}
break;
case commander_state_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */
@ -849,6 +858,22 @@ bool set_nav_state(struct vehicle_status_s *status,
break;
case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
/* must be rotary wing plus same requirements as normal landing */
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
}
break;
case commander_state_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */