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

Merge branch 'master' into ekf_voting

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-05 17:41:21 +02:00
parent 9950d5e950 329f7c0130
commit d532e372ac
54 changed files with 1091 additions and 243 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+14 -11
View File
@@ -1220,13 +1220,17 @@ int commander_thread_main(int argc, char *argv[])
}
// Run preflight check
int32_t rc_in_off = 0;
param_get(_param_autostart_id, &autostart_id);
if (is_hil_setup(autostart_id)) {
// HIL configuration selected: real sensors will be disabled
status.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
@@ -1242,7 +1246,6 @@ int commander_thread_main(int argc, char *argv[])
int32_t datalink_loss_enabled = false;
int32_t datalink_loss_timeout = 10;
float rc_loss_timeout = 0.5;
int32_t rc_in_off = 0;
int32_t datalink_regain_timeout = 0;
/* Thresholds for engine failure detection */
@@ -1863,17 +1866,16 @@ int commander_thread_main(int argc, char *argv[])
}
/* RC input check */
if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
mavlink_log_info(mavlink_fd, "Detected radio control");
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_info(mavlink_fd, "RC SIGNAL REGAINED after %llums",
mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums",
(hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
status_changed = true;
}
@@ -1913,8 +1915,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 == vehicle_status_s::ARMING_STATE_STANDBY &&
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* we check outside of the transition function here because the requirement
@@ -1925,13 +1926,15 @@ int commander_thread_main(int argc, char *argv[])
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
print_reject_arm("NOT ARMING: Configuration error");
}
stick_on_counter = 0;
@@ -1978,8 +1981,8 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000);
if (!status.rc_input_blocked && !status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
status.rc_signal_lost = true;
status.rc_signal_lost_timestamp = sp_man.timestamp;
status_changed = true;