fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 15:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'rc_timeout' into mpc_rc

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-13 11:06:49 +04:00
parent dfd9601b57 0b97dd2b77
commit 88149311ea
31 changed files with 858 additions and 696 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+66 -73
View File
@@ -67,6 +67,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
@@ -112,8 +113,7 @@ extern struct system_load_s system_load;
#define MAVLINK_OPEN_INTERVAL 50000
#define STICK_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_LIMIT 0.9f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
@@ -208,7 +208,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
@@ -861,6 +861,11 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info;
memset(&info, 0, sizeof(info));
/* Subscribe to position setpoint triplet */
int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
struct position_setpoint_triplet_s pos_sp_triplet;
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -1090,6 +1095,13 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
}
/* update subsystem */
orb_check(pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* compute system load */
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
@@ -1181,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1199,7 +1211,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1239,11 +1251,8 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
/* fill status according to mode switches */
check_mode_switches(&sp_man, &status);
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status);
res = set_main_state_rc(&status, &sp_man);
/* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
@@ -1254,6 +1263,33 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
/* set navigation state */
/* RETURN switch, overrides MISSION switch */
if (sp_man.return_switch == SWITCH_POS_ON) {
/* switch to RTL if not already landed after RTL and home position set */
status.set_nav_state = NAV_STATE_RTL;
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
/* MISSION switch */
if (sp_man.mission_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
status.set_nav_state_timestamp = hrt_absolute_time();
} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
/* stick is in MISSION position */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
}
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
@@ -1301,7 +1337,7 @@ int commander_thread_main(int argc, char *argv[])
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
}
@@ -1541,76 +1577,29 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
leds_counter++;
}
void
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
{
/* main mode switch */
if (!isfinite(sp_man->mode_switch)) {
/* default to manual if signal is invalid */
status->mode_switch = MODE_SWITCH_MANUAL;
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
status->mode_switch = MODE_SWITCH_AUTO;
} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
status->mode_switch = MODE_SWITCH_MANUAL;
} else {
status->mode_switch = MODE_SWITCH_ASSISTED;
}
/* return switch */
if (!isfinite(sp_man->return_switch)) {
status->return_switch = RETURN_SWITCH_NONE;
} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
status->return_switch = RETURN_SWITCH_RETURN;
} else {
status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
if (!isfinite(sp_man->assisted_switch)) {
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
status->assisted_switch = ASSISTED_SWITCH_EASY;
} else {
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
}
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
status->mission_switch = MISSION_SWITCH_LOITER;
} else {
status->mission_switch = MISSION_SWITCH_MISSION;
}
}
transition_result_t
set_main_state_rc(struct vehicle_status_s *status)
set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
{
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
switch (status->mode_switch) {
case MODE_SWITCH_MANUAL:
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case SWITCH_POS_OFF: // MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_ASSISTED:
if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
case SWITCH_POS_MIDDLE: // ASSISTED
if (sp_man->assisted_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_EASY);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT
print_reject_mode(status, "EASY");
@@ -1618,29 +1607,33 @@ set_main_state_rc(struct vehicle_status_s *status)
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
if (sp_man->assisted_switch != SWITCH_POS_ON) {
print_reject_mode(status, "SEATBELT");
}
// else fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_AUTO:
case SWITCH_POS_ON: // AUTO
res = main_state_transition(status, MAIN_STATE_AUTO);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT (EASY likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);