zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed

sbg
Anton Babushkin 12 years ago
parent
55fd19f281
commit
7c1693a877
3 changed files with 33 additions and 31 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 39
      src/modules/commander/commander.cpp
  2. 24
      src/modules/commander/state_machine_helper.cpp
  3. 1
      src/modules/uORB/topics/vehicle_control_mode.h

39
src/modules/commander/commander.cpp
Unescape View File

@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[])
}
}
/*
* Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
*/
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) {
/* Check if left stick is in lower left position and we're in manual mode --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
* TODO allow disarm when landed
*/
if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f &&
control_mode.flag_control_manual_enabled &&
((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) {
if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) {
mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
tune_negative();
} else {
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
tune_positive();
}
} else {
mavlink_log_critical(mavlink_fd, "DISARM not allowed");
tune_negative();
}
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
tune_positive();
stick_off_counter = 0;
} else {
@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check if left stick is in lower right position and we're in manual --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT &&
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
/* check if left stick is in lower right position and we're in manual mode --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd);
stick_on_counter = 0;

24
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
}
break;
@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
}
}
@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
}
break;
@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_manual_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current @@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}

1
src/modules/uORB/topics/vehicle_control_mode.h
Unescape View File

@ -77,6 +77,7 @@ struct vehicle_control_mode_s @@ -77,6 +77,7 @@ struct vehicle_control_mode_s
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */
bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */

Write Preview
Loading…
Cancel
Save