Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Commander: Switch into right mode in various corner conditions

Browse Source 
* If you move in LAND, AUTO or HOLD the sticks the system will give control back to the pilot
  * If you do not connect any RC the system will default to HOLD and will allow you tablet control
  * If you gain position lock for the first time the system will re-evaluate the mode switch (so if you dropped down to alt hold it will now go into position)
  * If the system breaches the Geofence it will now always drop back to POSCTRL if the sticks are moved
This commit is contained in:
Lorenz Meier
2017-02-04 21:05:28 +01:00
parent c8fb21e73e
commit 070a73ad63
2 changed files with 67 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+52 -6
View File
@@ -194,6 +194,7 @@ static float max_ekf_dang_bias = 3.5e-4f;
/* pre-flight IMU consistency checks */
static float max_imu_acc_diff = 0.7f;
static float max_imu_gyr_diff = 0.09f;
static float min_stick_change = 0.25f;
static struct vehicle_status_s status = {};
static struct vehicle_roi_s _roi = {};
@@ -229,6 +230,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was
static float avionics_power_rail_voltage; // voltage of the avionics power rail
static bool can_arm_without_gps = false;
static bool position_lock_gained = false;
/**
@@ -1083,7 +1085,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Taking off");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@@ -1298,6 +1302,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST");
param_t _param_min_stick_change = param_find("COM_RC_STICK_OV");
param_t _param_eph = param_find("COM_HOME_H_T");
param_t _param_epv = param_find("COM_HOME_V_T");
param_t _param_geofence_action = param_find("GF_ACTION");
@@ -1773,6 +1778,9 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
param_get(_param_rc_arm_hyst, &rc_arm_hyst);
param_get(_param_min_stick_change, &min_stick_change);
// percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1
min_stick_change *= 0.02f;
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
@@ -2498,15 +2506,34 @@ int commander_thread_main(int argc, char *argv[])
main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE ||
main_state_before_rtl == commander_state_s::MAIN_STATE_STAB)) {
// transition to previous state if sticks are increased
const float min_stick_change = 0.2f;
// transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
main_state_transition(&status, main_state_before_rtl, main_state_prev, &status_flags, &internal_state);
// revert to position control in any case
main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
mavlink_log_critical(&mavlink_log_pub, "Aborted RTL, returned control to pilot");
}
}
// abort landing or auto or loiter if sticks are moved significantly
if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL) {
// transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) {
// revert to position control in any case
main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
mavlink_log_critical(&mavlink_log_pub, "Autopilot off, returned control to pilot");
}
}
@@ -2850,6 +2877,19 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check if we are disarmed and there is a better mode to wait in */
if (!armed.armed) {
/* if there is no radio control but GPS lock the user might want to fly using
* just a tablet. Since the RC will force its mode switch setting on connecting
* we can as well just wait in a hold mode which enables tablet control.
*/
if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
&& status_flags.condition_home_position_valid) {
(void)main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
}
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -2924,12 +2964,17 @@ int commander_thread_main(int argc, char *argv[])
/* First time home position update - but only if disarmed */
if (!status_flags.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
position_lock_gained = true;
}
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
else if (((!was_armed && armed.armed) || (was_landed && !land_detector.landed)) &&
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
position_lock_gained = false;
} else {
position_lock_gained = false;
}
was_armed = armed.armed;
@@ -3267,7 +3312,7 @@ set_main_state_rc(struct vehicle_status_s *status_local)
// feature, just in case offboard control goes crazy.
/* manual setpoint has not updated, do not re-evaluate it */
if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) ||
if ((((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) ||
((_last_sp_man.offboard_switch == sp_man.offboard_switch) &&
(_last_sp_man.return_switch == sp_man.return_switch) &&
(_last_sp_man.mode_switch == sp_man.mode_switch) &&
@@ -3275,7 +3320,8 @@ set_main_state_rc(struct vehicle_status_s *status_local)
(_last_sp_man.rattitude_switch == sp_man.rattitude_switch) &&
(_last_sp_man.posctl_switch == sp_man.posctl_switch) &&
(_last_sp_man.loiter_switch == sp_man.loiter_switch) &&
(_last_sp_man.mode_slot == sp_man.mode_slot))) {
(_last_sp_man.mode_slot == sp_man.mode_slot)))
&& !position_lock_gained) {
// update these fields for the geofence system
src/modules/commander/commander_params.c
+15
View File
@@ -178,6 +178,21 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
*/
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
/**
* RC stick override threshold
*
* If an RC stick is moved more than by this amount the system will interpret this as
* override request by the pilot.
*
* @group Commander
# @unit %
* @min 5
* @max 40
* @decimal 0
* @increment 0.05
*/
PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 12.0f);
/**
* Home set horizontal threshold
*