diff options
author | Julian Oes <julian@oes.ch> | 2014-07-02 15:15:26 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-07-02 15:15:26 +0200 |
commit | 73d6121a9dbcd438f407edcd9b58bbb31bd94a42 (patch) | |
tree | cd82ecf355e68b80db6403716b394737f45f5231 /src/modules/commander/commander.cpp | |
parent | fdceb8b0620c347c9f7f477dbf295dcfff12012c (diff) | |
parent | f428ebb04f0610c31639d8fe6d121f632c1cad1b (diff) | |
download | px4-firmware-73d6121a9dbcd438f407edcd9b58bbb31bd94a42.tar.gz px4-firmware-73d6121a9dbcd438f407edcd9b58bbb31bd94a42.tar.bz2 px4-firmware-73d6121a9dbcd438f407edcd9b58bbb31bd94a42.zip |
Merge branch 'master' into offboard2_merge
Conflicts:
src/modules/uORB/topics/rc_channels.h
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 72 |
1 files changed, 63 insertions, 9 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ff99e7baf..38297745d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include <fcntl.h> #include <errno.h> #include <systemlib/err.h> +#include <systemlib/circuit_breaker.h> #include <debug.h> #include <sys/prctl.h> #include <sys/stat.h> @@ -76,6 +77,7 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/differential_pressure.h> #include <uORB/topics/safety.h> +#include <uORB/topics/system_power.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/telemetry_status.h> @@ -373,16 +375,16 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -513,7 +515,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -708,6 +717,12 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = hrt_absolute_time(); + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = -1.0f; + + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -760,7 +775,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -853,6 +867,11 @@ int commander_thread_main(int argc, char *argv[]) struct position_setpoint_triplet_s pos_sp_triplet; memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* Subscribe to system power */ + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + struct system_power_s system_power; + memset(&system_power, 0, sizeof(system_power)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -869,6 +888,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; + bool system_checked = false; while (!thread_should_exit) { @@ -910,6 +930,9 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); + + status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ @@ -922,6 +945,15 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } + /* Perform system checks (again) once params are loaded and MAVLink is up. */ + if (!system_checked && mavlink_fd && + (telemetry.heartbeat_time > 0) && + (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { + + (void)rc_calibration_check(mavlink_fd); + system_checked = true; + } + orb_check(sp_man_sub, &updated); if (updated) { @@ -965,6 +997,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); } + orb_check(system_power_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + status.condition_power_input_valid = false; + } else { + status.condition_power_input_valid = true; + } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; + } + } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); /* update safety topic */ @@ -1274,12 +1326,13 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 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("#audio: NOT ARMING: Press safety switch first."); - } else if (status.main_state != MAIN_STATE_MANUAL) { + /* we check outside of the transition function here because the requirement + * for being in manual mode only applies to manual arming actions. + * the system can be armed in auto if armed via the GCS. + */ + if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); - } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1309,6 +1362,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + tune_negative(true); } /* evaluate the main state machine according to mode switches */ |