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

Merge pull request #1216 from PX4/arming_feedback 

Arming feedback
This commit is contained in:
Lorenz Meier 2014-07-20 16:17:29 +02:00
parent 4c0e229347 a5f538dc5c
commit 2685e3cfa4
2 changed files with 23 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1407,8 +1407,12 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true; arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) { } else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */ /*
mavlink_log_critical(mavlink_fd, "arming state transition denied"); * the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true); tune_negative(true);
} }

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

@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
transition_result_t ret = TRANSITION_DENIED; transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state; arming_state_t current_arming_state = status->arming_state;
bool feedback_provided = false;
/* only check transition if the new state is actually different from the current one */ /* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) { if (new_arming_state == current_arming_state) {
@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if pre-arm check fails // Fail transition if pre-arm check fails
if (prearm_ret) { if (prearm_ret) {
/* the prearm check already prints the reject reason */
feedback_provided = true;
valid_transition = false; valid_transition = false;
// Fail transition if we need safety switch press // Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) { } else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false; valid_transition = false;
} }
@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (!status->condition_power_input_valid) { if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false; valid_transition = false;
} }
@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
(status->avionics_power_rail_voltage < 4.9f))) { (status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false; valid_transition = false;
} }
} }
@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current
/* Sensors need to be initialized for STANDBY state */ /* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
feedback_provided = true;
valid_transition = false; valid_transition = false;
} }
@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current
} }
if (ret == TRANSITION_DENIED) { if (ret == TRANSITION_DENIED) {
static const char *errMsg = "INVAL: %s - %s"; const char * str = "INVAL: %s - %s";
/* only print to console here by default as this is too technical to be useful during operation */
warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); /* print to MAVLink if we didn't provide any feedback yet */
if (!feedback_provided) {
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
}
} }
return ret; return ret;
@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
mavlink_log_critical(mavlink_fd, "hold still while arming");
/* this is frickin' fatal */ /* this is frickin' fatal */
failed = true; failed = true;
goto system_eval; goto system_eval;