diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 22e659ea41..a1ea14e2fe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -60,7 +60,6 @@ #include #include #include -//#include #include #include #include @@ -123,12 +122,6 @@ #include "esc_calibration.h" #include "PreflightCheck.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ /* Decouple update interval and hysteresis counters, all depends on intervals */ @@ -154,19 +147,6 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 -enum MAV_MODE_FLAG { - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ - MAV_MODE_FLAG_ENUM_END = 129, /* | */ -}; - - /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = 0; @@ -268,9 +248,6 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, - struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); - transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); /** @@ -745,7 +722,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_LAND: main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state); break; - case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET: + case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, main_state_prev, &status_flags, &internal_state); break; @@ -1209,15 +1186,6 @@ int commander_thread_main(int argc, char *argv[]) // main_states_str[commander_state_s::MAIN_STATE_STAB] = "STAB"; // main_states_str[commander_state_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; - // const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; - // arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; - // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; - // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; - // arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - // arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - // arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; - // arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - // const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; // nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; // nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 85f2ef590e..b3e9de3aba 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,6 @@ * @file px4_custom_mode.h * PX4 custom flight modes * - * @author Anton Babushkin */ #ifndef PX4_CUSTOM_MODE_H_ @@ -62,7 +61,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, - PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET + PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET }; union px4_custom_mode { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 28f61da576..8839c91b2a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -130,11 +130,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->nav_state) { + const uint8_t auto_mode_flags = MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + switch (status->nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; @@ -149,60 +152,52 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st break; case vehicle_status_s::NAVIGATION_STATE_STAB: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; // TODO: is POSCTL GUIDED? custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; @@ -212,17 +207,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_DESCEND: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= auto_mode_flags; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; break; @@ -233,9 +224,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, uint8_t *mavlink_st break; case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; break;