dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add new stabilize mode

This commit is contained in:
Lorenz Meier 2015-05-17 12:58:44 +02:00
parent 2f80ddd11a
commit 8e935e6fa6
4 changed files with 92 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/vehicle_status.msg
View File

@ -7,7 +7,8 @@ uint8 MAIN_STATE_AUTO_LOITER = 4
uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_AUTO_RTL = 5
uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_ACRO = 6
uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_OFFBOARD = 7
uint8 MAIN_STATE_MAX = 8 uint8 MAIN_STATE_STAB = 8
uint8 MAIN_STATE_MAX = 9
# If you change the order, add or remove arming_state_t states make sure to update the arrays # If you change the order, add or remove arming_state_t states make sure to update the arrays
# in state_machine_helper.cpp as well. # in state_machine_helper.cpp as well.
@ -39,7 +40,8 @@ uint8 NAVIGATION_STATE_LAND = 11 # Land mode
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_OFFBOARD = 14
uint8 NAVIGATION_STATE_MAX = 15 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode
uint8 NAVIGATION_STATE_MAX = 16
# VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128

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

@ -37,9 +37,9 @@
* *
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io> * @author Lorenz Meier <lorenz@px4.io>
* @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Thomas Gubler <thomas@px4.io>
* @author Julian Oes <julian@oes.ch> * @author Julian Oes <julian@px4.io>
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton@px4.io>
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -88,7 +88,7 @@
#include <uORB/topics/geofence_result.h> #include <uORB/topics/geofence_result.h>
#include <uORB/topics/telemetry_status.h> #include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/vehicle_land_detected.h>
#include <drivers/drv_led.h> #include <drivers/drv_led.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -497,6 +497,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* ACRO */ /* ACRO */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */ /* OFFBOARD */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
@ -514,6 +518,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* STABILIZED */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
} else {
/* MANUAL */ /* MANUAL */
main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
} }
@ -838,6 +845,7 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO";
main_states_str[vehicle_status_s::MAIN_STATE_STAB] = "STAB";
main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD";
const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
@ -851,6 +859,7 @@ int commander_thread_main(int argc, char *argv[])
const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; 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_MANUAL] = "MANUAL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
@ -1737,7 +1746,10 @@ int commander_thread_main(int argc, char *argv[])
* do it only for rotary wings */ * do it only for rotary wings */
if (status.is_rotary_wing && if (status.is_rotary_wing &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
(status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
status.condition_landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@ -1770,7 +1782,8 @@ int commander_thread_main(int argc, char *argv[])
* for being in manual mode only applies to manual arming actions. * for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS. * the system can be armed in auto if armed via the GCS.
*/ */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else { } else {
@ -1939,6 +1952,7 @@ int commander_thread_main(int argc, char *argv[])
* and both failed we want to terminate the flight */ * and both failed we want to terminate the flight */
if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) || ((status.data_link_lost && status.gps_failure) ||
@ -1963,6 +1977,7 @@ int commander_thread_main(int argc, char *argv[])
* if both failed we want to terminate the flight */ * if both failed we want to terminate the flight */
if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) || ((status.rc_signal_lost && status.gps_failure) ||
@ -2290,6 +2305,17 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
} else {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
} else if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_MIDDLE) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
} else { } else {
@ -2401,6 +2427,18 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false; control_mode.flag_control_termination_enabled = false;
break; break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true; control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false; control_mode.flag_control_auto_enabled = false;

42
src/modules/commander/px4_custom_mode.h
View File

@ -1,8 +1,41 @@
/* /****************************************************************************
* px4_custom_mode.h
* *
* Created on: 09.08.2013 * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
* Author: ton *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4_custom_mode.h
* PX4 custom flight modes
*
* @author Anton Babushkin <anton@px4.io>
*/ */
#ifndef PX4_CUSTOM_MODE_H_ #ifndef PX4_CUSTOM_MODE_H_
@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_OFFBOARD,
PX4_CUSTOM_MAIN_MODE_STABILIZED
}; };
enum PX4_CUSTOM_SUB_MODE_AUTO { enum PX4_CUSTOM_SUB_MODE_AUTO {

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

@ -299,6 +299,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
switch (new_main_state) { switch (new_main_state) {
case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_STAB:
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
break; break;
@ -488,6 +489,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
switch (status->main_state) { switch (status->main_state) {
case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_ACRO:
case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_MANUAL:
case vehicle_status_s::MAIN_STATE_STAB:
case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_ALTCTL:
case vehicle_status_s::MAIN_STATE_POSCTL: case vehicle_status_s::MAIN_STATE_POSCTL:
/* require RC for all manual modes */ /* require RC for all manual modes */
@ -514,6 +516,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break; break;
case vehicle_status_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break; break;