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

State machine rewritten, compiles, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-07-28 23:12:16 +04:00
parent 7c1693a877
commit 8c1067a017
5 changed files with 944 additions and 1183 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+16 -39
View File
@@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
}
/* autonomous mode */
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
}
@@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
if (v_status.main_state == MAIN_STATE_MANUAL
|| v_status.main_state == MAIN_STATE_SEATBELT
|| v_status.main_state == MAIN_STATE_EASY) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
if (v_status.navigation_state == MAIN_STATE_EASY) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
@@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* set calibration state */
if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY;
// XXX difference between active and armed? is AUTO_READY active?
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
} else if (v_status.arming_state == ARMING_STATE_INIT
|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (v_status.arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
} else {
warnx("Unknown mavlink state");
*mavlink_state = MAV_STATE_CRITICAL;
}
}