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

cleanup px4_custom_mode

Browse Source
This commit is contained in:
Daniel Agar
2016-06-15 20:35:16 +02:00
committed by Lorenz Meier
parent d16daf5ba4
commit 7bbfa5d94b
3 changed files with 29 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+1 -33
View File
@@ -60,7 +60,6 @@
#include <systemlib/err.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/mavlink_log.h>
//#include <debug.h>
#include <sys/stat.h>
#include <string.h>
#include <math.h>
@@ -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";
src/modules/commander/px4_custom_mode.h
+2 -3
View File
@@ -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 <anton@px4.io>
*/
#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 {
src/modules/mavlink/mavlink_messages.cpp
+26 -35
View File
@@ -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;